/**
 * @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)
    {
        // 计算公式为
        // Ref = ( 区间UQ差 / 区间调速电压差) * (VSP - 调速下限) + 区间最小UQ值
        if (mcFocCtrl.SREFValue <= 1408)
        { mcFocCtrl.Ref = _Q15(0.120); }
        else if (mcFocCtrl.SREFValue <= 11640 )
        { mcFocCtrl.Ref = 0.864 * (mcFocCtrl.SREFValue -  1408) + _Q15(0.120); }
        else if (mcFocCtrl.SREFValue <= 15530 )
        { mcFocCtrl.Ref = 1.238 * (mcFocCtrl.SREFValue - 11640) + _Q15(0.390); }
        else if (mcFocCtrl.SREFValue <= 19480 )
        { mcFocCtrl.Ref = 1.543 * (mcFocCtrl.SREFValue - 15530) + _Q15(0.537); }
        else if (mcFocCtrl.SREFValue <= 21962 )
        { mcFocCtrl.Ref = 1.755 * (mcFocCtrl.SREFValue - 19480) + _Q15(0.723); }
        else if (mcFocCtrl.SREFValue <= 23368 )
        { mcFocCtrl.Ref = 2.004 * (mcFocCtrl.SREFValue - 21962) + _Q15(0.856); }
        else
        { mcFocCtrl.Ref = _Q15(0.942); }
    }
    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
}

/**
    @brief        外部闭环控制函数，示例代码提供 电流环，速度环，功率环，UQ控制示例代码,可根据需要自行修改
                 建议使用默认1ms周期运行
    @date         2022-07-14
*/
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.LoopCalcValue = mcFocCtrl.UqFlt * 0.65 + mcFocCtrl.Power * 0.61;
                    
                        // 限幅
                        if (mcFocCtrl.LoopCalcValue > 32440)
                        { mcFocCtrl.LoopCalcValue = 32440;}
                    
                        mcFocCtrl.IqRef = HW_One_PI(refRampOut - mcFocCtrl.LoopCalcValue);
                        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 爬坡结束
    }
}