/**
    @copyright (C) COPYRIGHT 2022 Fortiortech Shenzhen
    @file      AddFunction.c
    @author    Fortiortech  Appliction Team
    @since     Create:2022-07-13
    @date      Last modify:2022-07-14
    @note      Last modify author is Marcel He
    @brief     This file contains main function used for Motor Control.
*/

#include <MyProject.h>

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

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

/**
    @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;
    }
}

/**
    @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 TargetRef_Process(void)
{
    #if (SPEED_MODE == PWMMODE)
    {
        PWMDutyCal();
        mcFocCtrl.PowerLimitValue = POWERLPFLIMIT;
    }
    #elif (SPEED_MODE == SREFMODE)
    {
        VSPSample();
        mcFocCtrl.PowerLimitValue = POWERLPFLIMIT;
    }
    #elif (SPEED_MODE == NONEMODE)
    {
        mcFocCtrl.PowerLimitValue = POWERLPFLIMIT;
        isCtrlPowOn = 1; // 开机
        mcFocCtrl.Ref = S_Value(1500);
    }
    #elif (SPEED_MODE == ONOFFTEST)
    {
        ONOFF_Test();
    }
    #endif
}

/**
    @brief        外部闭环控制函数，示例代码提供 电流环，速度环，功率环，UQ控制示例代码,可根据需要自行修改
                 建议使用默认1ms周期运行
    @date         2022-07-14
*/
void Speed_response(void)
{
    static int16 refRampOut = 0;
    
    if ((mcState == mcRun) || (mcState == mcStop))
    {
        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)
						mcRefRamp.OutValue_float = mcFocCtrl.SpeedFlt;
						#elif (MOTOR_CTRL_MODE == POWER_LOOP_CONTROL)
                        mcRefRamp.OutValue_float = mcFocCtrl.PowerFlt;
						#elif (MOTOR_CTRL_MODE == UQ_LOOP_CONTROL)
						mcRefRamp.OutValue_float = mcFocCtrl.UqFlt;
						#endif
                        mcFocCtrl.LoopTime = LOOP_TIME;
                        mcRefRamp.IncValue = RAMP_INC;
                        mcRefRamp.DecValue = 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 = Motor_Ramp(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.PowerFlt); // 限制功率
                    
                        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.PowerFlt);
                        FOC_IQREF = mcFocCtrl.IqRef;
                    }
                    #elif (MOTOR_CTRL_MODE == UQ_LOOP_CONTROL)
                    {
                        mcFocCtrl.IqRef = HW_One_PI(refRampOut - mcFocCtrl.UqFlt);
                        FOC_IQREF = mcFocCtrl.IqRef;
                    }
                    #else
                    {
                    }
                    #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 Motor_Ramp(int16 ref)
{
    mcRefRamp.RefValue = ref; // 爬坡函数输入
    
    if (mcRefRamp.OutValue_float < mcRefRamp.RefValue)
    {
        if (mcRefRamp.OutValue_float + mcRefRamp.IncValue < mcRefRamp.RefValue)
        {
            mcRefRamp.OutValue_float += mcRefRamp.IncValue;
        }
        else
        {
            mcRefRamp.OutValue_float = mcRefRamp.RefValue;
        }
    }
    else
    {
        if (mcRefRamp.OutValue_float - mcRefRamp.DecValue > mcRefRamp.RefValue)
        {
            mcRefRamp.OutValue_float -= mcRefRamp.DecValue;
        }
        else
        {
            mcRefRamp.OutValue_float = mcRefRamp.RefValue;
        }
    }
    
    return (int16)mcRefRamp.OutValue_float; // 输出浮点数取整
}

/**
    @brief        启动ATO爬坡函数，用于静止启动时候对ATO进行爬坡，提高启动可靠性
    @date         2022-07-14
*/
void ATORamp(void)
{
    if (mcState == mcRun)
    {
        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 爬坡结束
        }
    }
}

/**
    @brief        默认1ms周期服务函数，运行信号采样，调速信号处理，闭环控制，故障检测,ATO爬坡函数
                 该函数运行于大循环中，由SYSTICK定时器间隔1ms触发运行。
    @date         2022-07-14
*/
void TickCycle_1ms(void)
{
    #if (VoltageCompensationEn == 1)
    {
        if (VoltageComp.cpscnt <= VoltageCompensationDelayCnt)
        { SetBit(ADC_CR, ADCBSY);}
    }
    #else
	// 使能ADC的DCBUS采样
	SetBit(ADC_CR, ADCBSY); 
	#endif
    
    if ((mcState != mcInit) && (mcState != mcReady))
    {
        // 速度滤波
        mcFocCtrl.SpeedFlt = LPFFunction(FOC__EOME, mcFocCtrl.SpeedFlt, 50); // 注意低通滤波器系数范围为0---127
        mcFocCtrl.EMFsquare = Sqrt_alpbet(FOC__EALP, FOC__EBET);
        
        if (mcState == mcRun)
        {
            mcFocCtrl.Power = FOC__POW << 3; // 功率滤波
            mcFocCtrl.PowerFlt = LPFFunction(mcFocCtrl.Power, mcFocCtrl.PowerFlt, 10);
        }
    }
    else
    {
        mcFocCtrl.SpeedFlt = 0;
        mcFocCtrl.PowerFlt = 0;
    }
    
    // DCbus的采样获取值并滤波
    #if (VoltageCompensationEn == 1)
    {
		// 未进入电压补偿之前在1ms中断里面采集电压，进入之后在载波中断里面采集电压
        if (VoltageComp.cpscnt <= VoltageCompensationDelayCnt)
        { mcFocCtrl.mcDcbusFlt = LPFFunction(ADC2_DR, mcFocCtrl.mcDcbusFlt, 50); }
        
        if ((mcFocCtrl.CtrlMode == 1) && (mcState == mcRun) && (mcFaultSource == FaultNoSource)) // 进入速度环后进行延时计数
        {
            if (VoltageComp.cpscnt < VoltageCompensationDelayCnt + 10)
            { VoltageComp.cpscnt++; }
            else
            { VoltageComp.cpscnt = VoltageCompensationDelayCnt + 10; }
        }
        else
        { VoltageComp.cpscnt = 0; }
        
        // 母线电压平均值
        BusAverageVoltage();
    }
    #else
	mcFocCtrl.mcDcbusFlt = LPFFunction(ADC2_DR, mcFocCtrl.mcDcbusFlt, 50); // 母线电压值滤波
    #endif
    mcFocCtrl.NTCValue = ADC7_DR;
    mcFocCtrl.NTCValueFlt = LPFFunction(mcFocCtrl.NTCValue, mcFocCtrl.NTCValueFlt, 60);
    mcFocCtrl.UqFlt = LPFFunction(FOC__UQ, mcFocCtrl.UqFlt, 50);
    mcFocCtrl.UdFlt = LPFFunction(FOC__UD, mcFocCtrl.UdFlt, 50);
    // 获取调速信号，不同调速模式(PWMMODE,NONEMODE,SREFMODE)的目标值修改
    TargetRef_Process();
    // 启动ATO控制，环路响应，如速度环、转矩环、功率环等
    Speed_response();
    //故障保护函数功能，如过欠压保护、启动保护、缺相、堵转等
    Fault_Detection();
    // 电机启动ATO爬坡函数处理
    ATORamp();
    
    // 电机状态机的时序处理
    if (mcFocCtrl.State_Count > 0)
    { mcFocCtrl.State_Count--; }
}