motor_ctl-4/User project/1.FrameLayer/Source/TimeTask_Event.c

/**
 * @file TimeTask_Event.c
 * @author Zhang, Kai(zhangkai71@midea.com)
 * @brief Time task control
 * @version 0.1
 * @date 2021-09-27
 *
 * @copyright Copyright (c) 2021
 *
 */
/******************************
 *
 *  Included File
 *
 ******************************/
#include "TimeTask_Event.h"
#include "AssistCurve.h"
#include "FSM_1st.h"
#include "FSM_2nd.h"
#include "FuncLayerAPI.h"
#include "can.h"
#include "canAppl.h"
#include "flash_master.h"
#include "queue.h"
#include "gd32f30x.h"
#include "string.h"
#include "syspar.h"
#include "user.h"
#include "STLmain.h"
/******************************
 *
 *  Parameter
 *
 ******************************/
_op_ Op[proc_cnt] = {{Event_5ms, EVE1MSCNT_5ms, EVE1MSCNT_5ms},
                     {Event_10ms, EVE1MSCNT_10ms, EVE1MSCNT_10ms},
                     {Event_20ms, EVE1MSCNT_20ms, EVE1MSCNT_20ms},
                     {Event_100ms, EVE1MSCNT_100ms, EVE1MSCNT_100ms},
                     {Event_200ms, EVE1MSCNT_200ms, EVE1MSCNT_200ms}};

static SQWORD TimingTaskTimerTick = 0;
static SQWORD TimingTaskTimerTickTemp = 0;
static SQWORD TimingTaskTimerTickTempOld = 0;
static SQWORD TimingTaskTimerTickPassed = 0;
static UWORD  LoopServerExecutedFlag = 0;
static UWORD  AssistCNT = 0;
UWORD socTest = 100;
BOOL tstMafClrFlg = FALSE;
/******************************
 *
 *  Function
 *
 ******************************/
void  Event_1ms(void)
{
    /* 1st FSM control */
    FSM_1st_Main();
    FSM1st_Sys_state.Event_hook();
    
    /* Power control */
//    power_voPowerManagement(cp_stFlg.ParaHistorySaveEEFinishFlg, cp_stFlg.ParaSaveEEFlg);
    power_voPowerManagement(ass_ParaCong.uwAutoPowerOffTime * 60, cp_ulSystickCnt, OBC_ButtonStatus.ulButtonSetTimeCnt, \
                            MC_RunInfo.Torque, MC_RunInfo.Cadence, MC_RunInfo.BikeSpeed, \
                            cp_stFlg.ParaHistorySaveEEFinishFlg, cp_stFlg.ParaSaveEEFlg);
    
    /* cp_history info update */
    Can_voMC_Run_1ms();

    /* Torque move average filter */
    if (cadence_stFreGetOut.uwForwardCnt > 0)
    {
        torsensor_voCadenceCnt();
        cadence_stFreGetOut.uwForwardCnt = 0;
        maf_torque.value = torsensor_stTorSensorOut.uwTorquePu;
        MoveAverageFilter(&maf_torque);
        
        //////Iqref maf test, dont add torq obs//////
        if(tstTorqPIFlg == 1)
        {
            maf_uqlimit.value = ass_stTorqPIOut.swIRefPu;
            MoveAverageFilter(&maf_uqlimit);
            tstMafClrFlg = FALSE;
        }
        else if((tstTorqPIFlg == 0) && (tstMafClrFlg == FALSE))
        {
            MoveAverageFilterClear(&maf_uqlimit);
            tstMafClrFlg = TRUE;
        }    
        
    }
    /* Torque info update */
    torsensor_voTorADC();
//    torsensor_voDynamicOffset();
//    torsensor_voTorADCwithTemp();

    /* Torque assist calculation*/
//    ass_CalIn.swFlxIqLimit = MC_RunInfo.SOC;
    ass_CalIn.SOCValue = socTest;
    if(cp_stFlg.RunModelSelect == CityBIKE )
    {
        ass_CalIn.swDirection = -1;
    }
    else if(cp_stFlg.RunModelSelect == MountainBIKE)
    {
        ass_CalIn.swDirection = 1;
    }
    else
    {
        ass_CalIn.swDirection = 1;
    }
    ass_CalIn.swFlxIqLimit = abs(flx_stCtrlOut.swIqLimPu);
    ass_CalIn.swPwrIqLimit = abs(pwr_stPwrLimOut2.swIqLimPu);
    ass_CalIn.uwbikespeed = bikespeed_stFreGetOut.uwLPFFrequencyPu;
    ass_CalIn.uwcadancelast = ass_CalIn.uwcadance;
    ass_CalIn.uwcadance = cadence_stFreGetOut.uwLPFFrequencyPu;
    ass_CalIn.uwcadancePer = cadence_stFreGetOut.uwFreqPercent;
    ass_CalIn.uwcadanceFWCnt = cadence_stFreGetOut.uwForwardCnt;
    ass_CalIn.uwGearSt = (cp_stBikeRunInfoPara.uwBikeGear <= 6) ? cp_stBikeRunInfoPara.uwBikeGear : 0;
    ass_CalIn.uwSpdFbkAbsPu = scm_uwSpdFbkLpfAbsPu;
    ass_CalIn.swSpdFbkPu = scm_stSpdFbkLpf.slY.sw.hi;
    ass_CalIn.uwBaseSpdrpm = cof_uwVbRpm;
    ass_CalIn.uwtorque = maf_torque.AverValue; // maf_torque.AverValue;//torsensor_stTorSensorOut.uwTorqueLPFPu;
    ass_CalIn.uwtorquelpf = torsensor_stTorSensorOut.uwTorqueLPFPu;
    ass_CalIn.uwtorquePer = torsensor_stTorSensorOut.uwTorquePu;
    ass_CalIn.swCurFdbPu = scm_swIqFdbLpfPu;
    ass_CalIn.swCurRefPu = scm_swIqRefPu;
    Assist();
    
    uart_swTorqRefNm = Assist_torqueper;
    
      /////////test///////////
//    if((cp_stFlg.RunModelSelect == CityBIKE) || (cp_stFlg.RunModelSelect == MountainBIKE))
//    {
//            
//      ass_CalOut.swVoltLimitPu = scm_swVsDcpLimPu;
//       uart_swTorqRefNm = ass_CalIn.swDirection * ass_ParaSet.uwSpeedAssistIMaxA;
////      if(MC_ControlCode.GearSt == 0x01)
////      {
////          uart_swTorqRefNm = ass_CalIn.swDirection * 546;
////      }
////      else if(MC_ControlCode.GearSt == 0x02)
////      {
////          uart_swTorqRefNm = ass_CalIn.swDirection * 546 * 2;
////      }
////      else if(MC_ControlCode.GearSt == 0x03)
////      {
////          uart_swTorqRefNm = ass_CalIn.swDirection * 546 * 3;     
////      }
////      else if(MC_ControlCode.GearSt == 0x04)
////      {
////          uart_swTorqRefNm = ass_CalIn.swDirection * 546 * 4;  
////      }
////      else if(MC_ControlCode.GearSt == 0x33 || MC_ControlCode.GearSt == 0x05)
////      {
////          uart_swTorqRefNm = ass_CalIn.swDirection * 546 * 5;
////      }
////      else
////      {
////          uart_swTorqRefNm = 0;
////      }
//      
//      if(uart_swTorqRefNm != 0)
//      {
//         ass_CalCoef.blAssistflag = TRUE;
//      }
//      else
//      {
//         ass_CalCoef.blAssistflag = FALSE;
//      }
//          
//    }
    

    /* Torque assist mode flag */
    if (ass_CalCoef.blAssistflag == TRUE && cp_stFlg.RunPermitFlg == TRUE && cp_stFlg.SpiOffsetFirstSetFlg ==1)
    {
        signal_state.Sensor = TRUE;
    }
    else if( cp_stFlg.SpiOffsetFirstSetFlg == 0 && cp_stFlg.RunPermitFlg == TRUE && cp_stFlg.SpiOffsetFirstSetFinishFlg == FALSE)
    {
        signal_state.Sensor = TRUE;
    }
    else
    {
        signal_state.Sensor = FALSE;
    }

    /* Speed assist mode flag */
    if((cp_stFlg.RunModelSelect == CityBIKE) || (cp_stFlg.RunModelSelect == MountainBIKE))
    {
        #if 0  // By throttle ADC signal
        if(bikethrottle_stBikeThrottleOut.uwThrottlePercent < 500)
        {
            AssistCNT ++;
            if(AssistCNT > 1000 && cp_stFlg.RunPermitFlg == TRUE)
            {
                signal_state.Assist = TRUE;
                AssistCNT = 1000;
            }
        }
        else
        {   
            AssistCNT = 0;
            signal_state.Assist = FALSE;
        }
        #elif 0 // By walk assist GPIO signal
        
        #elif 1 //By can bus command code
        if(cp_stBikeRunInfoPara.uwBikeGear == 0x22)
        {
            AssistCNT ++;
            if(AssistCNT > 200 && cp_stFlg.RunPermitFlg == TRUE)
            {
                signal_state.Assist = TRUE;
                AssistCNT = 200;
            }
        }
        else
        {
            AssistCNT = 0;
            signal_state.Assist = FALSE;
        }
        #endif

        if(signal_state.Assist == TRUE)
        {
            //6km/H = 100m/min  1.67m/s
            if(cp_stFlg.RunModelSelect == CityBIKE)
            {
                uart_slSpdRefRpm = -(10000/(ass_ParaCong.uwWheelDiameter))*ass_ParaCong.uwNmBackChainring*ass_ParaCong.uwMechRationMotor/ass_ParaCong.uwNmFrontChainring;
//                cp_stBikeRunInfoPara.BikeSpeedKmH = 60;      //constant display of 6km/h
            }
            else if(cp_stFlg.RunModelSelect == MountainBIKE)
            {
                uart_slSpdRefRpm = (10000/(ass_ParaCong.uwWheelDiameter))*ass_ParaCong.uwNmBackChainring*ass_ParaCong.uwMechRationMotor/ass_ParaCong.uwNmFrontChainring;
//                cp_stBikeRunInfoPara.BikeSpeedKmH = 60;      //constant display of 6km/h
            }
            else
            {
            }

        }
        else
        {
            uart_slSpdRefRpm = 0;
        }
    }
    else
    {          
        if ((uart_slSpdRefRpm >= 10 || uart_slSpdRefRpm <= -10) && cp_stFlg.RunPermitFlg == TRUE )
        {
            signal_state.Assist = TRUE;
        }
        else
        {
            signal_state.Assist = FALSE;
        }
    }  
}

void Event_5ms(void)
{
    Can_voMC_Run_5ms();
}

void Event_10ms(void)
{
    /* Throttle ADC voltage update */
    bikethrottle_voBikeThrottleADC();

    /*  Speed command set */
    if(cp_stFlg.RunModelSelect != CityBIKE && cp_stFlg.RunModelSelect != MountainBIKE )
    {
        /* Use instrument */
//        Signal_detect();
      
        /* Use upper computer */
        if(cp_stFlg.RotateDirectionSelect == ForwardRotate)
        {
            uart_slSpdRefRpm = ((SLONG)MC_MotorSPD_rpm_Percent*5000)/100;
        }
        else if(cp_stFlg.RotateDirectionSelect == BackwardRotate)
        {
            uart_slSpdRefRpm = -((SLONG)MC_MotorSPD_rpm_Percent*5000)/100;
        }
     
        /* bikespeed closeloop control  */
//         bikemotorspdref();
//         ass_CalIn.swDirection =-1;       
//         uart_slSpdRefRpm = bikeSpdPIOut.swSpdRefRpm * ass_CalIn.swDirection;

         if(abs(uart_slSpdRefRpm) < 300)
         {
             uart_slSpdRefRpm = 0;
         }

    }
    
    /* Bike light control */
    Can_Light_switch();
    bikelight_voBikeLightControl(cp_stBikeRunInfoPara.uwLightSwitch);

    /* Trip cal when open */
    bikespeed_votempTripCal();

}


void  Event_20ms(void)
{
    stl_voDoRunTimeChecks();
}

void Event_100ms(void)
{
    bikespeed_stFreGetOut.uwLPFFrequencyPu = (bikespeed_stFreGetOut.uwLPFFrequencyPu * bikespeed_stFreGetCof.uwBikeSpeedLPFGain +
                                              bikespeed_stFreGetOut.uwFrequencyPu * (100 - bikespeed_stFreGetCof.uwBikeSpeedLPFGain)) /
                                             100;
    

}

UWORD ReadorWrite = 2;
void  Event_200ms(void)
{
  
 // SendData(ID_MC_BC, MODE_REPORT, 0x1020, (uint8_t *)&MC_RunInfo.BikeSpeed);
    if (ReadorWrite == 3)
    {
        stErrorLog.NotesInfo1 = 1;
        stErrorLog.NotesInfo2 = 2;
        stErrorLog.NotesInfo3 = 3;
        que_ubPushIn(&stFlashErrorLog, &stErrorLog, sizeof(stErrorLog));
        ReadorWrite = 2;
    }

    Can_voMC_Run_200ms();

    bikelight_voGetBikeLightError(adc_stUpOut.uwU6VPu);
    display_voGetDisplayError(adc_stUpOut.uwU12VPu);
    bikespeed_voGetBikeSpeedPwrError(adc_stUpOut.uwU5VPu);

    /* Bike faults 200ms detect */
//    if((cp_stFlg.RunModelSelect == MountainBIKE) || (cp_stFlg.RunModelSelect == CityBIKE))
//    {
//        alm_stBikeIn.uwTroqReg = torsensor_stTorSensorOut.uwTorqueReg;
//        alm_stBikeIn.uwTroqPu = torsensor_stTorSensorOut.uwTorqueLPFPu;//torsensor_stTorSensorOut.uwTorquePu;
//        alm_stBikeIn.blBikeSpdOvrFlg = bikespeed_stFreGetOut.blBikeSpeedSensorPwrErrorFlg;
//        alm_stBikeIn.blCadenceFreqOvrFlg = cadence_stFreGetOut.blCadenceSensorErrorFlg;
//        alm_stBikeIn.swMotorSpdDir = ass_CalIn.swDirection;
//        alm_stBikeIn.swMotorSpdPu = scm_stSpdFbkLpf.slY.sw.hi;
//        alm_stBikeIn.uwBikeSpdPu = bikespeed_stFreGetOut.uwFrequencyPu;
//        alm_stBikeIn.uwCadenceFreqPu = cadence_stFreGetOut.uwFrequencyPu;
//        alm_stBikeIn.uwMotorNTCReg = adc_stUpOut.MotorTempReg;
//        alm_stBikeIn.uwPCBNTCReg = adc_stUpOut.PCBTempReg;
//        alm_stBikeIn.uwThrottleReg = adc_stUpOut.uwThrottleReg;
//        alm_stBikeIn.blThrottleExistFlg = FALSE;
//        alm_stBikeIn.blMotorNTCExistFlg = FALSE;
//        alm_voDetec200MS(&alm_stBikeIn, &alm_stDetect200MSCoef);   
//    }
    
    if (switch_flg.SysFault_Flag == TRUE)
    {
        SendData(ID_MC_BC, MODE_REPORT, 0x1104, (uint8_t *)&MC_ErrorCode.Code);
    }
}

 void Signal_detect(void)
{
    SWORD sign;

    if (MOTOR_ID_SEL == MOTOR_WELLING_MTB)
    {
        sign = 1;
    }
    else if(MOTOR_ID_SEL == MOTOR_WELLING_CITY)
    {
        sign = -1;
    }
    else
    {
        sign=1;
    }         
         
    if(MC_ControlCode.GearSt == 0x01)
    {
        uart_slSpdRefRpm = sign *785;
    }
    else if(MC_ControlCode.GearSt == 0x02)
    {
        uart_slSpdRefRpm = sign *1000;
    }
    else if(MC_ControlCode.GearSt == 0x03)
    {
        uart_slSpdRefRpm = sign *3088;      
    }
    else if(MC_ControlCode.GearSt == 0x04)
    {
        uart_slSpdRefRpm = sign*3603;      
    }
    else if(MC_ControlCode.GearSt == 0x33 || MC_ControlCode.GearSt == 0x05)
    {
        uart_slSpdRefRpm = sign*4500;
    }
    else
    {
        uart_slSpdRefRpm = 0;
    }

}

void TimingTaskTimerServer(void)
{
    TimingTaskTimerTick++;
    LoopServerExecutedFlag = 0;
}

void TimingTaskLoopServer(void)
{
    SLONG cnt;
    TimingTaskTimerTickTempOld = TimingTaskTimerTickTemp;
    TimingTaskTimerTickTemp = TimingTaskTimerTick;
    TimingTaskTimerTickPassed = TimingTaskTimerTickTemp - TimingTaskTimerTickTempOld;
    if (LoopServerExecutedFlag == 0)
    {
        for (cnt = 0; cnt < proc_cnt; cnt++)
        {
            Op[cnt].tick -= TimingTaskTimerTickPassed;
            if (Op[cnt].tick <= 0)
            {
                Op[cnt].tick += Op[cnt].timespan;
                Op[cnt].proc();
            }
        }
        LoopServerExecutedFlag = 1;
    }
}