


/**
 ******************************************************************************
 *
 * @file        MG32_BLDC_API.c
 * @brief       Drive BLDC c Code. 
 *
 * @par         Project
 *              MG32
 * @version     V1.02
 * @date        2021/05/28
 * @author      Megawin Software Center
 * @copyright   Copyright (c) 2017 MegaWin Technology Co., Ltd.
 *              All rights reserved.
 *
 ******************************************************************************* 
 * @par Disclaimer
 * The Demo software is provided "AS IS" without any warranty, either
 * expressed or implied, including, but not limited to, the implied warranties
 * of merchantability and fitness for a particular purpose. The author will
 * not be liable for any special, incidental, consequential or indirect
 * damages due to loss of data or any other reason.
 * These statements agree with the world wide and local dictated laws about
 * authorship and violence against these laws.
 *******************************************************************************
 *******************************************************************************
 */


/* Includes ------------------------------------------------------------------*/
#include "MG32_BLDC_API.h"
#include "MG32_APB_MID.h"
#include "MG32_BLDC_SinewaveTable.h"
#include "MG32_BLDC_System.h"

/* Wizard menu ---------------------------------------------------------------*/
/* Private typedef -----------------------------------------------------------*/
/* Private define ------------------------------------------------------------*/
/* Private macro -------------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
#if defined (__ARMCC_VERSION) && (__ARMCC_VERSION >= 6010050) /* ARM Compiler V6 */
    #pragma clang diagnostic push
    #pragma clang diagnostic ignored "-Wmissing-variable-declarations"
        TM_HandleTypeDef  mTM36;
        TM_HandleTypeDef  hTM16;
        TM_OC_InitTypeDef sConfig;
    #pragma clang diagnostic pop
#else
    TM_HandleTypeDef  mTM36;
    TM_HandleTypeDef  hTM16;
    TM_OC_InitTypeDef sConfig;
#endif

// PID control
static volatile int32_t  Verror, Acci;
static volatile int32_t  p, e;

// PWM output pattern
#if defined (__ARMCC_VERSION) && (__ARMCC_VERSION >= 6010050) /* ARM Compiler V6 */
    #pragma clang diagnostic push
    #pragma clang diagnostic ignored "-Wmissing-variable-declarations"
        volatile const uint8_t PWMpattern_R []= 
        {				
            // REVERSE
            // Top side 'or' Bottom side of H-bridge
            TM_PRELOADOC00_ENABLE | TM_PRELOADOC11_ENABLE,  //    AT + BB
            TM_PRELOADOC02_ENABLE | TM_PRELOADOC11_ENABLE,  //    CT + BB
            TM_PRELOADOC02_ENABLE | TM_PRELOADOC10_ENABLE,  //    CT + AB
            TM_PRELOADOC01_ENABLE | TM_PRELOADOC10_ENABLE,  //    BT + AB
            TM_PRELOADOC01_ENABLE | TM_PRELOADOC12_ENABLE,  //    BT + CB
            TM_PRELOADOC00_ENABLE | TM_PRELOADOC12_ENABLE,  //    AT + CB
        };
    #pragma clang diagnostic pop
#else
    volatile const uint8_t PWMpattern_R []= 
    {				
        // REVERSE
        // Top side 'or' Bottom side of H-bridge
        TM_PRELOADOC00_ENABLE | TM_PRELOADOC11_ENABLE,  //    AT + BB
        TM_PRELOADOC02_ENABLE | TM_PRELOADOC11_ENABLE,  //    CT + BB
        TM_PRELOADOC02_ENABLE | TM_PRELOADOC10_ENABLE,  //    CT + AB
        TM_PRELOADOC01_ENABLE | TM_PRELOADOC10_ENABLE,  //    BT + AB
        TM_PRELOADOC01_ENABLE | TM_PRELOADOC12_ENABLE,  //    BT + CB
        TM_PRELOADOC00_ENABLE | TM_PRELOADOC12_ENABLE,  //    AT + CB
    };
#endif
 
// HALL pattern        
#if defined (__ARMCC_VERSION) && (__ARMCC_VERSION >= 6010050) /* ARM Compiler V6 */
    #pragma clang diagnostic push
    #pragma clang diagnostic ignored "-Wmissing-variable-declarations"
        const uint16_t HALLpattern_R[] = 
        { 
            (0x0050 << 3),
            (0x0040 << 3),
            (0x0060 << 3),
            (0x0020 << 3),
            (0x0030 << 3),
            (0x0010 << 3)
        };
    #pragma clang diagnostic pop
#else
    const uint16_t HALLpattern_R[] = 
    { 
        (0x0050 << 3),
        (0x0040 << 3),
        (0x0060 << 3),
        (0x0020 << 3),
        (0x0030 << 3),
        (0x0010 << 3)
    };
#endif

#if defined (__ARMCC_VERSION) && (__ARMCC_VERSION >= 6010050) /* ARM Compiler V6 */
    #pragma clang diagnostic push
    #pragma clang diagnostic ignored "-Wmissing-variable-declarations"
        volatile const uint8_t PWMpattern_F []= 
        {				
            // FORWARD
            // Top side 'or' Bottom side of H-bridge
            (TM_PRELOADOC01_ENABLE | TM_PRELOADOC10_ENABLE),  //    BT + AB   
            (TM_PRELOADOC02_ENABLE | TM_PRELOADOC10_ENABLE),  //    CT + AB
            (TM_PRELOADOC02_ENABLE | TM_PRELOADOC11_ENABLE),  //    CT + BB
            (TM_PRELOADOC00_ENABLE | TM_PRELOADOC11_ENABLE),  //    AT + BB
            (TM_PRELOADOC00_ENABLE | TM_PRELOADOC12_ENABLE),  //    AT + CB
            (TM_PRELOADOC01_ENABLE | TM_PRELOADOC12_ENABLE),  //    BT + CB
        };
    #pragma clang diagnostic pop
#else
    volatile const uint8_t PWMpattern_F []= 
    {				
        // FORWARD
        // Top side 'or' Bottom side of H-bridge
        (TM_PRELOADOC01_ENABLE | TM_PRELOADOC10_ENABLE),  //    BT + AB   
        (TM_PRELOADOC02_ENABLE | TM_PRELOADOC10_ENABLE),  //    CT + AB
        (TM_PRELOADOC02_ENABLE | TM_PRELOADOC11_ENABLE),  //    CT + BB
        (TM_PRELOADOC00_ENABLE | TM_PRELOADOC11_ENABLE),  //    AT + BB
        (TM_PRELOADOC00_ENABLE | TM_PRELOADOC12_ENABLE),  //    AT + CB
        (TM_PRELOADOC01_ENABLE | TM_PRELOADOC12_ENABLE),  //    BT + CB
    };
#endif

#if defined (__ARMCC_VERSION) && (__ARMCC_VERSION >= 6010050) /* ARM Compiler V6 */
    #pragma clang diagnostic push
    #pragma clang diagnostic ignored "-Wmissing-variable-declarations"
        const uint16_t HALLpattern_F[] = 
        { 
            (0x0050 << 3),
            (0x0010 << 3),
            (0x0030 << 3),
            (0x0020 << 3),
            (0x0060 << 3),
            (0x0040 << 3)
        };
    #pragma clang diagnostic pop
#else
    const uint16_t HALLpattern_F[] = 
    { 
        (0x0050 << 3),
        (0x0010 << 3),
        (0x0030 << 3),
        (0x0020 << 3),
        (0x0060 << 3),
        (0x0040 << 3)
    };
#endif

/* Private function prototypes -----------------------------------------------*/
void API_BLDC_Square_Init(void);
void TM3x_IRQHandler(void);
void API_BLDC_Square_STOP (void);
void API_BLDC_Square_ReverseRotating (void);
int32_t saturate (int32_t i, int32_t l);
void PI_Cal_KpKi_routine (void);
void PI_Calculate (void);

/* Exported variables --------------------------------------------------------*/
/* Exported functions --------------------------------------------------------*/
/* External vairables --------------------------------------------------------*/
extern volatile Motor_HandlerDef hMotor;



    
    
/**
 *******************************************************************************
 * @brief       Initial TM36 PWM output & Sine Wave Table
 * @param[in]   None
 * @return		None
 *******************************************************************************
 */
void API_BLDC_Square_Init(void)
{
    volatile uint16_t CNT_Rot;    
    TM_IC_InitTypeDef iConfig;
    TM_ClockConfigTypeDef CKConfig;

    
    // ------------------------------------------------------------------------
    // check Hall pattern
    // ------------------------------------------------------------------------
    if ((API_BLDC_GET_HALL() == 0x0000) || (API_BLDC_GET_HALL() == (0x0070 << 3)))
    {
        hMotor.MotorState = Motor_Hall_Fault;
        return;
    }
    
    // ------------------------------------------------------------------------
    // TM16 confiuration
    // ------------------------------------------------------------------------
    // TM16 init for counting time of MOTOR rotation
    hTM16.Instance = TM16;
    hTM16.Init.TM_CounterMode = TM_CASCADE_UP;
    hTM16.Init.TM_Period = 65535;
    hTM16.Init.TM_Prescaler = (SineSteps - 1);
    MID_TM_Base_Init(&hTM16);
        
    CKConfig.TM_ClockSource         = TM_INTERNAL_CLOCK;
    CKConfig.TM_InternalClockSource = TM_INTERNALCLOCK_PROC;
    CKConfig.TM_INTClockDivision    = TM_INTERNALCLOCK_DIVDER_DIV1; 
#if defined ( __GNUC__ )
    CKConfig.TM_ExternalClockSource = 0;
#else
    CKConfig.TM_ExternalClockSource = NULL;
#endif
    MID_TM_ConfigClockSource(&hTM16, &CKConfig);    
        
    MID_TM_Base_Start(&hTM16);
    
    // ------------------------------------------------------------------------
    // TM36 confiuration
    // ------------------------------------------------------------------------
    // initial TM36 for 10-bit resolution of PWM
    mTM36.Instance = TM36;
    mTM36.Init.TM_CounterMode = TM_CASCADE_UP;
    mTM36.Init.TM_Period = 1023;
    mTM36.Init.TM_Prescaler = 0;
    MID_TM_Base_Init(&mTM36);
   
    MID_TM_ConfigClockSource(&mTM36, &CKConfig);    
    // ------------------------------------------------------------------------
    // Pin initial for PWM (Channel-0 & 1)
    MID_TM_OC_Struct_Init(&sConfig);
    sConfig.OCMode = TM_CH_16bit_PWM;
    sConfig.Pulse = 1024;                                           // duty cycle = 100%
    MID_TM_PWM_ConfigChannel(&mTM36, &sConfig, MID_TM_Channel0);    // Top side of H-bridge : output 100%

    sConfig.Pulse = INIT_DUTY;
    MID_TM_PWM_ConfigChannel(&mTM36, &sConfig, MID_TM_Channel1);    // Low side of H-bridge : output PWM
    
    // ------------------------------------------------------------------------
    // Set Break in cycle-by-cycle mode
    __DRV_TM_SET_BREAK_MODE(&mTM36, TM_BK_CYCLE_BY_CYCLE);
    // Enable Break function for TM36 PWM output
    __DRV_TM_ENABLE_BREAK(&mTM36);
    // Select Break source from Pin, Clock Failure, CPU lockup, BOD1 event
    __DRV_TM_ENABLE_BREAKSOURCE(&mTM36, MID_BK_ExtPin | MID_BK_ClockFailure | MID_BK_CPULOCKUP | MID_BK_BOD1);
    
    // Enable TM36 Break IT
    __DRV_TM_ENABLE_IT(&mTM36, TM_IT_BREAK);

    // ------------------------------------------------------------------------
    // Enable PWM output 
    MID_TM_PWM_Start(&mTM36, MID_TM_Channel0);          // top-side of H-bridge  
    MID_TM_PWM_Start(&mTM36, MID_TM_Channel1);          // low-side of H-bridge
    
    // ------------------------------------------------------------------------
    // initial TM36_XOR pin
    iConfig.ICSelection = MID_TM_INPUTMUX_PIN;
    iConfig.ICPolarity  = TM_ICPOLARITY_DISABLE;
    MID_TM_IC_ConfigChannel(&mTM36, &iConfig, MID_TM_Channel0);
    MID_TM_IC_ConfigChannel(&mTM36, &iConfig, MID_TM_Channel1);
    MID_TM_IC_ConfigChannel(&mTM36, &iConfig, MID_TM_Channel2);
    
    iConfig.ICSelection = MID_TM_INPUTMUX_LINE3;        // TM36_XOR trigger signal connect IC channel
    iConfig.ICPolarity = TM_ICPOLARITY_DUALEDGE;

    MID_TM_IC_ConfigChannel(&mTM36, &iConfig, MID_TM_Channel3);
    MID_TM_IC_Start_IT(&mTM36, MID_TM_Channel3);   

    NVIC_EnableIRQ(TM3x_IRQn);
    NVIC_SetPriority(TM3x_IRQn, 0x00);                  // The Highest priority

    // ------------------------------------------------------------------------
    // TM36 preload function config 
    // ------------------------------------------------------------------------
    if(hMotor.Direction == MOTOR_FORWARD)
    {
        CNT_Rot=0;
        do
        {
            if(API_BLDC_GET_HALL() == HALLpattern_F[CNT_Rot])
            {
                __DRV_TM_SET_PRELOAD_OUTPUT(&mTM36, PWMpattern_F[CNT_Rot]);
                break;
            }
            
            CNT_Rot ++;
        } while(CNT_Rot<6);
        
    }
    else
    {
        CNT_Rot=0;
        do
        {
            if(API_BLDC_GET_HALL() == HALLpattern_R[CNT_Rot])
            {
                __DRV_TM_SET_PRELOAD_OUTPUT(&mTM36, PWMpattern_R[CNT_Rot]);
                break;
            }
            
            CNT_Rot ++;
        } while(CNT_Rot<6);
    }
    
    hMotor.Table_IDX =  CNT_Rot;

    // ------------------------------------------------------------------------
    // reset TM16 
    // ------------------------------------------------------------------------
    __DRV_TM_RESET_TIMER(&hTM16);

    // ------------------------------------------------------------------------
    // Startup : PWM
    // ------------------------------------------------------------------------
    // output PWM in Motor_Startup state.    
    hMotor.MotorState = Motor_Starup;
    
    __DRV_TM_SOFTWARE_PRELOAD(&mTM36);                // Software trigger preload event
    __DRV_TM_ENABLE_PRELOAD(&mTM36, TM_PRELOAD_3XOR); // preload event accpept 3-line XOR (Hall)

    //                                                    
    for(CNT_Rot=0; CNT_Rot < 24; CNT_Rot++)
    {
        MID_ClearTick();                            // clear Systick
        
        hMotor.commutate_flag = 0;
        while(1)
        {
            if(hMotor.commutate_flag == 1) break;   // wait for motor commutate

            if(MID_GetTick()  > 3000)               // Motor speed < 5rpm (That means stuck state happened)
            {
                API_BLDC_Square_STOP();                
                hMotor.MotorState = Motor_Stuck;
                return;
            }
            
            // speedup 
//            if(MID_GetTick() & 0x00000040)
//            {
//                hMotor.CurrentDuty ++;
//                __DRV_TM_SET_COMPARE_B(&mTM36, MID_TM_Channel1, hMotor.CurrentDuty);
//            }
        } 
        switch(API_BLDC_GET_HALL())
        {
        case (0x0050 << 3):
            hMotor.HallUsedPattern |= 0x01;
            break;
        case (0x0010 << 3):
            hMotor.HallUsedPattern |= 0x02;
            break;
        case (0x0030 << 3):
            hMotor.HallUsedPattern |= 0x04;
            break;
        case (0x0020 << 3):
            hMotor.HallUsedPattern |= 0x08;
            break;
        case (0x0060 << 3):
            hMotor.HallUsedPattern |= 0x10;
            break;
        case (0x0040 << 3):
            hMotor.HallUsedPattern |= 0x20;
            break;
        }
        
    }
    
    // check hall pattern
    if(hMotor.HallUsedPattern != 0x003F)            // Hall pattern is not {0x5, 0x4, 0x6, 0x2, 0x3, 0x1}
    {
        API_BLDC_Square_STOP();                
        hMotor.MotorState = Motor_Hall_Fault;
        return;
    }
    
    

    // ------------------------------------------------------------------------
    // Motor running
    // ------------------------------------------------------------------------
    hMotor.MotorState = Motor_Running;
    hMotor.commutate_time = 0;
    
}



//-----------------------------------------------------------------------------
// TM36 interrupt routine
//-----------------------------------------------------------------------------
void TM3x_IRQHandler(void)
{
    static uint16_t CNT6Step = 0;
    
    // XOR event
    if((TM36->STA.W & (TM_FLAG_CC3A | TM_FLAG_CC3B)) != 0x00)
    {
        // Get TM16 main counter value and replace TM10 counter
        if((++CNT6Step) > 11)
        {
            hMotor.MotorSpeed = __DRV_TM_GET_COUNTER(&hTM16) >> 1;
            CNT6Step = 0;
            
            // reset TM16 
            __DRV_TM_RESET_TIMER(&hTM16);
        }
                 
        // clear TM36 flag
        __DRV_TM_CLEAR_FLAG(&mTM36, (TM_FLAG_CC3A | TM_FLAG_CC3B));
        __DRV_TM_CLEAR_FLAG(&hTM16, TM_FLAG_UPDATE_UP);
        
        // setting commutate flag & record commutate time
        hMotor.commutate_flag = 1;
            
        // Modify next PWM output state
        hMotor.Table_IDX ++;
        if(hMotor.Table_IDX > 5)
            hMotor.Table_IDX = 0;
        
        if(hMotor.Direction == MOTOR_FORWARD)
            __DRV_TM_SET_PRELOAD_OUTPUT(&mTM36, PWMpattern_F[hMotor.Table_IDX]);
        else
            __DRV_TM_SET_PRELOAD_OUTPUT(&mTM36, PWMpattern_R[hMotor.Table_IDX]);
        
        return;
    }
    
    // Break event happened
    if(__DRV_TM_GET_FLAG(&mTM36, TM_FLAG_BREAK) != 0x00)
    {
        hMotor.MotorState = Motor_OverCurrent;
        __DRV_TM_CLEAR_FLAG(&mTM36, TM_FLAG_BREAK);
    }
    
        
}


//-----------------------------------------------------------------------------
// BLDC Stop running state
//-----------------------------------------------------------------------------
void API_BLDC_Square_STOP (void)
{
    // Stop TM36 PWM output
    __DRV_TM_DISABLE_IT(&mTM36, TM_IT_BREAK);           // disable TM36 break interrupt
    __DRV_TM_SOFTWARE_ENABLE_BREAK(&mTM36);             // software enable break state then TM36 all channel output stop state 
    
    __DRV_TM_SET_PRELOAD_OUTPUT(&mTM36, 0x00);          // set OC0x(x=0~2) & OC1x(x=0~2) output stop state     
    __DRV_TM_SOFTWARE_PRELOAD(&mTM36);                  // trigger TM36 output preload control
    
    // Disable IRQ handle
    NVIC_DisableIRQ(TM3x_IRQn);
    
    // DeInit all timer
    MID_TM_Base_Stop(&hTM16);
    MID_TM_Base_DeInit(&hTM16);
    
    MID_TM_Base_Stop(&mTM36);
    MID_TM_IC_Stop_IT(&mTM36, MID_TM_Channel3);   
    
    MID_TM_PWM_Stop(&mTM36, MID_TM_Channel0);
    MID_TM_PWM_Stop(&mTM36, MID_TM_Channel1);
    
    sConfig.OCMode = TM_CH_Disable;                 
    sConfig.OCIdleState = TM_OCIDLESTATE_RESET;         // output low    
    sConfig.OCNIdleState = TM_OCNIDLESTATE_RESET;       // output low    
    MID_TM_PWM_ConfigChannel(&mTM36, &sConfig, MID_TM_Channel0);

    MID_TM_PWM_ConfigChannel(&mTM36, &sConfig, MID_TM_Channel1);

    __DRV_TM_SOFTWARE_DISABLE_BREAK(&mTM36);            // exit TM36 break state
}


//-----------------------------------------------------------------------------
// BLDC Stop running state
//-----------------------------------------------------------------------------
void API_BLDC_Square_ReverseRotating (void)
{
    uint32_t CompareHallPattern;
    
    // Stop TM36 PWM output
    API_BLDC_Square_STOP();    
    
    // wait for motor halt
    CompareHallPattern = API_BLDC_GET_HALL();
    while(1)
    {
        if(CompareHallPattern != API_BLDC_GET_HALL())
        {
            hMotor.commutate_flag = 0;
            hMotor.commutate_time = MID_GetTick();
            CompareHallPattern = API_BLDC_GET_HALL();
        }
        if((MID_GetTick()- hMotor.commutate_time) > 500)
        {                
            break;
        }
    }
    
    MID_Delay(500);

    // Change Motor direction
    if(hMotor.Direction == MOTOR_REVERSE)
        hMotor.Direction = MOTOR_FORWARD;
    else
        hMotor.Direction = MOTOR_REVERSE;
    
}

//-----------------------------------------------------------------------------
// saturate()
//-----------------------------------------------------------------------------
int32_t saturate (int32_t i, int32_t l)
{
    if(i > +l)                         // if greater than upper limit
    {
        i = +l;                        // set to upper limit
    }
    else if (i < -l)                  // if less than lower limit
    {
        i = -l;                        // set to lower limit
    }
    return i;                         // return saturated value
}

//-----------------------------------------------------------------------------
// PID()
//-----------------------------------------------------------------------------
void PI_Cal_KpKi_routine ()
{
    int32_t T;
    
    e = 0;
    if(KPINIT!=0)                               // check for zero
    {
        p = Verror * KPINIT;
        p = saturate(p, KpStatution);
    }
    
    if(KIINIT!=0)                               // check for zero
    {
        Acci += Verror;
        Acci = saturate(Acci, KiStatution);     // avoid Acci too large
        
        T = Acci * KIINIT;
        T = saturate(T, KiStatution);
    }
    
    e = p + T;
}


//-----------------------------------------------------------------------------
// for caluate PI function
//-----------------------------------------------------------------------------
void PI_Calculate (void) 
{ 
    volatile int32_t tmp;
    
    Verror =  hMotor.DesiredDuty - hMotor.CurrentDuty;
    if(Verror == 0) return;
    
    PI_Cal_KpKi_routine ();

    e = e >> 5;
    if (e == 0) return;
    
    // motor speed can't lower the MIN_RPM
    if(hMotor.MinSpeed_flag == MOTOR_LOWER_MIN_RPM)
        if (e < 0) return;

    //
    tmp = hMotor.CurrentDuty + e;
    if(tmp < 0) tmp = 0;
    if(tmp > 1023) tmp = 1023;
    hMotor.CurrentDuty = tmp;
    __DRV_TM_SET_COMPARE_B(&mTM36, MID_TM_Channel1, tmp);
}


