/*******************************************************************************
*
* Freescale Semiconductor Inc.
* (c) Copyright 2013 Freescale Semiconductor, Inc.
* ALL RIGHTS RESERVED.
* 
* Do not modify the code!
*
****************************************************************************//*!
*
* @file     main.c
*
* @version  1.0.0.0
*
* @date     31-Jan-2013
*
* @brief    BLDC Motor Control Hall Sensor Based Application for MC9S12ZVML128,
*           mask set 0N95G. Using the latest release of MC lib.
*
***/



#include <hidef.h> /* for EnableInterrupts macro */
#include "derivative.h" /* include peripheral declarations */
#include "typedefs.h"
#include "mlib.h"
#include "gflib.h"
#include "SWLIBS_Config.h"
#include "gdflib.h"
#include "freemaster.h"

/*******************************************************************************
* Function prototypes
*******************************************************************************/
void InitPTU(void);
void InitADC(void);
void InitCPMU(void);
void InitPIM(void);
void InitPMF(void);
void InitGDU(void);
void InitTIM(void);
void InitINT(void);
void InitSCI(void);
void UpdateDutyCycle(void);
void GetKeyboard(void);

void AppInit(void);
void AppAlignment(void);
void AppStart(void);
void AppRun(void);
void AppStop(void);
void AppFault(void);

__declspec(interrupt) void ADC0done_ISR(void);
__declspec(interrupt) void ADC0error_ISR(void);
__declspec(interrupt) void TIMch1_ISR(void);
__declspec(interrupt) void TIMch3_ISR(void);


void FindHallPattern(void);

/*******************************************************************************
* Defines
*******************************************************************************/
// Internal clock 1MHz, 25/12.5 MHz CPU/Bus clock, 6.25 MHz ADC clock
#define _INTERNAL_CLOCK						// 1 MHz internal clock is used
#define	CPMU_REFDIV		0
#define	CPMU_SYNDIV		24
#define	CPMU_POSTDIV	1
#define	CPMU_REFFRQ		0
#define	CPMU_VCOFRQ		1
// ADC clock [0.25 - 8MHz] = fbus / ( 2 * (ADC_TIM + 1)) =  6.25 MHz ADC clock
#define ADC_TIM			0
// DC bus current sampling point calibration
// 20kHz PWM step = 50us / 1250 = 0.04us
// 9 * 0.04us = 0.36us
#define ADC_SAMPLE_TIME_CALIBRATION  9 
#define	MIN_ADC_TRIGGER_FIRST	8
#define	MIN_ADC_TRIGGER_SECOND	48
// PWM clock = core clock / PWM_PRSCA = 25MHz / 1 = 25MHz 
#define PWM_PRSCA       0  
 // PWM freq = PWM clock / PWM_MODULO = 12.5MHz / 1250 = 10kHz 
#define	PWM_MODULO		1250 
 // PWM dead time = (1 / PWM clock) * PWM_DEADTIME = (1 / 25MHz) * 13 = 0.52us
#define	PWM_DEADTIME	13  
// Timer prescaler 128; 12.5 MHz/128 = 10.24 us
#define TIM_PRESCALER	7 		
#define TIMER_1MS		98
// SCI baud rate 9.6kBd
// Formula changed comparing to 0N06E and 0N56G mask set.
// SCIBR = Bus clock / SCI_BAUDRATE = 12.5MHz / 1302 = 9.6kBd
#define	SCI_BAUDRATE	1302

// Application states
#define APP_INIT                0 
#define APP_ALIGNMENT           1
#define APP_START               2
#define APP_RUN                 3
#define APP_STOP                4
#define APP_FAULT               5

// Motor start duty cycle
#define START_DUTY_CYCLE   100

// Rotation direction
#define CW     0
#define CCW    1

// Control
#define OFF    0
#define ON     1

// PI controller parameters
#define RUN_PI_UPPER_LIMIT				0.96
#define RUN_PI_LOWER_LIMIT				0.04
#define RUN_PI_SPEED_PARAM_UP_CC1		0.01
#define RUN_PI_SPEED_PARAM_DOWN_CC1		0.003
#define RUN_PI_CURRENT_PARAM_CC1		0.05

// PWM duty cycle minimal value 0.08us * 50 = 4us 
#define PWM_DUTYCYCLE_MIN   50

// Current limitation threshold
/* Current sense OA reference voltage Uref = 2.5V
   ADC 5V max
   Power stage calculation numerator: 
   U_Rsense_max = (U_ADCmax - Uref)/ OA_Gain = (5V - 2.5V)/ 12.5 = 0.2V
   I_Rsense_max = U_Rsense_max / Rsense = 0.2V / 10mR = 20A
   DCBusCurrent max value: 2^15 - 1 = 32767 is equal to motor phase current 20A 
   
   Maximal current limit MOTOR_CURRENT_MAX = FRAC16(Motor phase current limit [A] / 20A))
   MOTOR_CURRENT_MAX = FRAC16(4Amp / 20Amp) = FRAC16(0.20) */
#define MOTOR_CURRENT_MAX                  FRAC16(0.2)
#define MOTOR_CURRENT_CALC_NUMERATOR       FRAC16(0.16666667)
// Set difference between actual and average motor phase current to detect possible stall to
// 50% of measured motor phase average current
#define MOTOR_PHASE_CURRENT_DIFFERENCE     FRAC16(0.5)

// Motor stall period threshold (period without detection of Hall signal edge)  
// 100 * 1ms = 100ms
#define MOTOR_STALL_1MS_COUNT    100

// Hall period without detecting Hall signal edge
// Maximal value = HallPeriodMax * TIMtick * 1ms = 10900 * 10.24us * 1ms = 110 * 1ms = 110ms
// Minimal measurable speed = SPEED_CALC_NUMERATOR / period6ZCmax = SPEED_CALC_NUMERATOR / ((HALL_EDGE_PERIOD_MAX / TIMtick) * 6) = 50rpm
#define HALL_EDGE_PERIOD_MAX     110

// 65535 equals to 25V
// 16V max
#define DC_BUS_OVERVOLTAGE_SET   16
#define DC_BUS_OVERVOLTAGE       ((DC_BUS_OVERVOLTAGE_SET * 65535) / 25) 
// 10V min
#define DC_BUS_UNDERVOLTAGE_SET  6
#define DC_BUS_UNDERVOLTAGE      ((DC_BUS_UNDERVOLTAGE_SET * 65535) / 25) 

// Application control LEDs
#define APPLICATON_CONTROL_LED1  PTT_PTT0 
#define FAULT_REPORT_LED2        PTS_PTS1 

// Application switches
#define SW1_UP      PTP_PTP1
#define SW2_DOWN    PTS_PTS0

// 1ms counter periods to read the SW1 and SW2
#define CONTER_PERIOD  1000

/*******************************************************************************
* Types Definition
*******************************************************************************/
// Pointer to a function
typedef void (*tPointerFcn)(void); 

typedef union 
{
    uint8_t byte;
    struct 
    {
        uint8_t DCBusOverCurrent:1;
        uint8_t DCBusOverVoltage:1;
        uint8_t DCBusUnderVoltage:1;
        uint8_t MotorStall : 1;
        uint8_t HallPaternError : 1;
        uint8_t Reserved:2;
    }bit;
}tFaultStatus;

typedef union uAppControl
  {
	uint8_t  byte;
    struct
    {
	  uint8_t StopRun          :1; // Stop - 0, Run - 1
	  uint8_t RotDirection     :1; // CW - 0, CCW - 1
	  uint8_t ClearFault       :1; // No - 0, Yes - 1
	  uint8_t bit3             :1;
	  uint8_t bit4             :1;
	  uint8_t bit5             :1;
	  uint8_t bit6             :1;
	  uint8_t bit7             :1;
    }bit;
}tAppControl;

/*******************************************************************************
* Variables definition
*******************************************************************************/
// Array with pointers to the state machine functions
static tPointerFcn AppStateMachine[] = \
{
    AppInit,
    AppAlignment,
    AppStart,
    AppRun,
    AppStop,
    AppFault
};

// Commutation control
volatile uint16_t dutyCycle;
volatile uint16_t cmtSector;
volatile uint16_t hallPattern;

// Commutation period measurement
volatile uint16_t  timerValue;
volatile uint16_t  timerLastValue;
volatile uint16_t  timerValueLim;

// Application control
volatile tAppControl appControl;
volatile tFaultStatus faultStatus;
// 0 - Stop, 1 - Run
volatile uint8_t motorStatus;
volatile  uint8_t  appState;
volatile  uint8_t  rotDir;
volatile uint16_t  hallEdgePerod1msCounter;

// Control speed and current loop
volatile uint16_t period6ZC;
volatile uint16_t timerValue, timerLastValue, timerValueLim;
tFrac16 currentErr, motorCurrentLimit, motorCurrentDifference, motorAverageCurrentDifference, motorCurrentCounter, motorCurrentFiltered;
GDFLIB_FILTER_IIR1_T_F16  f16motorCurrentIIR1 = GDFLIB_FILTER_IIR1_DEFAULT_F16;
tFrac16 speedErr, requiredSpeed;
tFrac32 actualSpeed;
tFrac16 speedPIOut, currentPIOut;
GFLIB_CONTROLLER_PIAW_R_T_F16 speedPIPrms, currentPIPrms;
volatile uint16_t dutyCycle;


// Hall period array
uint16_t hallEdgePeriod[7] = {0, 0, 0, 0, 0, 0, 0};

// Analog values measurement
tU16 DCBusVoltage;
tFrac16  DCBusCurrentOffset, DCBusCurrent;

// Torque
tFrac16 motorCurrent6ZC, motorCurrent, motorCurrentError;

// Array torque
tFrac16 motorHallEdgePeriodCurrent[7] = {0, 0, 0, 0, 0, 0, 0};

// 1ms counter
tU16 counter1ms, nextSwCtrlPeriod;

// Motor stall period counter
tU16 motorStallCounter;

// Motor speed control by on board switches
tU16 switchMotorSpeedCtrl;

volatile uint16_t delay;
volatile uint16_t changeSector;
volatile uint16_t updatePWM;
volatile uint16_t NextCmtSector;

/**************************************************************************************************
    PTU and ADC data placed in APP_DATA_RAM segment 256Bytes long defined in mc9s12zvml128.prm file
/**************************************************************************************************
 *  Trigger event list
 *
 *  +-------------------+ - (int)PTUTriggerEventList)
 *  +===================+ - Trigger 0 space
 *  +*******************+ - List 0 offset = &PTUTriggerEventList[0][0][0] - &PTUTriggerEventList[0][0][0] = 0
 *  | DelayT0           |
 *  +-------------------+
 *  | DelayT1           |
 *  +-------------------+
 *  | 0x0000            | - End of delay List 0
 *  +*******************+ - List 1 offset = &PTUTriggerEventList[0][1][0] - &PTUTriggerEventList[0][0][0] = 3
 *  | DelayT0           |
 *  +-------------------+
 *  | DelayT1           |
 *  +-------------------+
 *  | 0x0000            | - End of delay List 1
 *  +===================+ - Trigger 1 space
 *  +*******************+ - List 0 offset = &PTUTriggerEventList[1][0][0] - &PTUTriggerEventList[0][0][0] = 6
 *  | DelayT0           |
 *  +-------------------+
 *  | DelayT1           |
 *  +-------------------+
 *  | 0x0000            | - End of delay List 0
 *  +*******************+ - List 1 offset = &PTUTriggerEventList[1][1][0] - &PTUTriggerEventList[0][0][0] = 9
 *  | DelayT0           |
 *  +-------------------+
 *  | DelayT1           |
 *  +-------------------+
 *  | 0x0000            | - End of delay List 1
 *  +===================+
 *
 *
 ***************************************************************************/
// PTU trigger list. Select array address, where address last bit value is 0 (see PTUPTR)
// PTUTriggerEventList[TG0, TG1][List 0, List 1][Delay T0 hex, DelayT1 hex, EOL symbol 0x0000]
// Default set-up: 
// 	PTUTriggerDCBI = 50hex => 30dec / bus clock = 30 / 12.5Mhz = 2.4us
//  PTUTriggerDCBV = 3E8hex => 500dec / bus clock = 500 / 12.5Mhz = 40us
volatile uint16_t PTUTriggerEventList[2][2][3] @0x00001000 = \
{ \
	// TG0  List 0, List 1	
	{{0x001E,0x01F4,0x0000},{0x001E,0x01F4,0x0000}},
	// TG1  List 0, List 1	
	{{0x0000,0x0000,0x0000},{0x0000,0x0000,0x0000}}
};


//  ADC0 Command List. Select array address, where address last 2 bits value is 0 (see ADC0CBP)
/*	CMD_SEL[1:0]:
	00 normal conversion
	40 end of sequence - wait for trigger to execute next sequence or restart 
	80 end of list - wrap to list top and continue
	C0 end of list - wrap to list top and stop
 */
// Command format {CMD_SEL + INTFLG_SEL, VREF + Channel, SMP, Reserved 
volatile uint8_t ADC0CommandList[6][4] @0x00001030 = \
{ \
	{0x40,0xD0,0x00,0x00}, // End of sequence, convert AN0 pin DC bus current, 4 clock cycles sample time
	{0xC0,0xCB,0x00,0x00}, // End of list, convert HD pin DC bus voltage, 4 clock cycles sample time
	{0x00,0x00,0x00,0x00},
	{0x00,0x00,0x00,0x00},
	{0x00,0x00,0x00,0x00},
    {0x00,0x00,0x00,0x00}
}; 

// ADC0 conversion results. Select array address, where address last 2 bits value is 0 (see ADC0RBP)
volatile uint16_t ADC0ResultList[6] @0x00001060 = {0, 0, 0, 0, 0, 0};

// ADC0 errors
volatile uint16_t AdcErrorLDOK, AdcErrorRSTAR, AdcErrorTRIG, AdcErrorEOL, AdcErrorCMD, AdcErrorIA;

/*******************************************************************************
* Constants definition
*******************************************************************************/
// BLDC motor PWM pattern
const uint8_t MaskVal[7]   = {0x34,0x1C, 0x13,0x31, 0x0D,0x07, 0x3F};
const uint8_t OutCtl[7]    = {0x0C,0x30, 0x30,0x03, 0x03,0x0C, 0x00};

// Hall pattern, clockwise and counter clockwise. Array first member is not used.
const uint8_t BLDCPatternBasedOnHall[2][7] = {{0, 0, 2, 1, 4, 5, 3},{0, 3, 5, 4, 1, 2, 0}};

// Motor speed control
#define SPEED_CALC_NUMERATOR            1464844     // (60 * fTIM) / motor pole pairs = 60 / (10.24us * 4) 
#define MIN_SPEED						300         // [rpm]
#define MAX_SPEED						8000		// [rpm]
#define SPEED_STEP						500			// [rpm]
#define INITIAL_SPEED                   2000 		// [rpm]
#define SPEED_ERROR_STEP_MAX           -1500		// [rpm]

/*****************************************************************************
*
* Function: void InitPTU(void)
*
* Description: Programmable Trigger Unit module configuration
*
*****************************************************************************/
void InitPTU(void)
{
	PTUPTRL = (uint8_t)((vuint32_t)&PTUTriggerEventList[0][0][0]);
	PTUPTRM = (uint8_t)(((vuint32_t)&PTUTriggerEventList[0][0][0]) >> 8);
	PTUPTRH = (uint8_t)(((vuint32_t)&PTUTriggerEventList[0][0][0]) >> 16);

	// Load TG0 list 1 offset. PTU delay list members are 16bits value, that's why the address difference result is divided by 2.
	TG0L1IDX = (uint8_t)(((vuint32_t)&PTUTriggerEventList[0][1][0] - (vuint32_t)&PTUTriggerEventList[0][0][0]) >> 1); 
	
	// Enable Trigger Generation 0
	PTUE_TG0EN = 1;
	  
}

/*****************************************************************************
*
* Function: void initAdc(void)
*
* Description: ADC module configuration
*
*****************************************************************************/
void InitADC(void)
{
	// ADC0
	 // Dual access mode vis internal interface and data bus
	ADC0CTL_0_ACC_CFG = 3; 
	// Store result at abort/restart
	ADC0CTL_0_STR_SEQA = 1;     

	// ADC clock 6.25 MHz 
	ADC0TIM = ADC_TIM; 
	
	// Left justified data result 
	ADC0FMT_DJM = 0; 
	// 12-bit data result
	ADC0FMT_SRES = 4;           
	// End of list interrupt enable
	ADC0CONIE_1_EOL_IE = 1;    

	// ADC0 Command Base Pointer
	ADC0CBP_0 = (uint8_t)(((vuint32_t)&ADC0CommandList[0][0]) >> 16);
	ADC0CBP_1 = (uint8_t)(((vuint32_t)&ADC0CommandList[0][0]) >> 8);
	ADC0CBP_2 = (uint8_t)((vuint32_t)&ADC0CommandList[0][0]);

	// ADC0 Result Base Pointer
	ADC0RBP_0 = (uint8_t)(((vuint32_t)&ADC0ResultList[0]) >> 16);
	ADC0RBP_1 = (uint8_t)(((vuint32_t)&ADC0ResultList[0]) >> 8);
	ADC0RBP_2 = (uint8_t)((vuint32_t)&ADC0ResultList[0]);

	// Enable ADC0
	ADC0CTL_0_ADC_EN = 1;  
	
	ADC0EIE = 0xEE;     // enable all errors interrupts
}

/*****************************************************************************
*
* Function: void InitCPMU(void)
*
* Description: Clock, Reset and Power Management Unit configuration
*
*****************************************************************************/
void InitCPMU(void)
{
	// Wait for stable supply after power up
	while (GDUF_GLVLSF)
	{
		GDUF_GLVLSF = 1;
	}

	CPMUREFDIV_REFDIV = CPMU_REFDIV;
	CPMUREFDIV_REFFRQ = CPMU_REFFRQ;
	CPMUSYNR_SYNDIV = CPMU_SYNDIV;
	CPMUSYNR_VCOFRQ = CPMU_VCOFRQ;
	CPMUPOSTDIV_POSTDIV = CPMU_POSTDIV;

	while (CPMUIFLG_LOCK == 0) 
	{}
	
	//Clear PORF and LVRF
	CPMURFLG  = 0x60; 	
}

/*****************************************************************************
*
* Function: void InitTIM(void)
*
* Description: Timer module configuration
*
*****************************************************************************/
void InitTIM(void)
{
	// TIM ch0 output compare
	TIM0TIOS_IOS0 = 1;
	// TIM ch1 input capture
	TIM0TIOS_IOS1 = 0;
	// TIM ch3 output compare
	TIM0TIOS_IOS3 = 1;     

	// Precision timer disabled
	TIM0TSCR1_PRNT = 0;
	
	// TIM ch0 output no action
	TIM0TCTL2_OL0 = 0;  
	TIM0TCTL2_OM0 = 0;
	
	// TIM ch1 input capture on any edge
	TIM0TCTL4_EDG1A = 1;
	TIM0TCTL4_EDG1B = 1;
	
	// TIM ch3 no action on an output compare
	TIM0TCTL2_OL3 = 0;  
	TIM0TCTL2_OM3 = 0;

	// Load prescaler value
  	TIM0TSCR2_PR = TIM_PRESCALER;
  	
  	// TIM ch3 interrupt every 1[ms]
  	TIM0TC3 = TIMER_1MS;  

  	// Disconnect all output compare pins
  	TIM0OCPD = 0xFF;  
  	
	// TIM ch1 interrupt enable
	TIM0TIE_C1I = 1;  
	// TIM ch3 interrupt enable
    TIM0TIE_C3I = 1;  
    
    // TIM global enable
  	TIM0TSCR1_TEN = 1;
}

/*****************************************************************************
*
* Function: void InitPIM(void)
*
* Description: I/O pins configuration
*
*****************************************************************************/
void InitPIM(void)
{
	// TIM0 input capture channel 1 is connected to logic XOR of PT1, PT2, PT3
	MODRR2_T0IC1RR = 1; 
	
	// Hall sensor PT1, PT2, PT3 pull-ups enabled 
	PERT = 0x0E;
	
	// Enable EVDD1
	// PP0 +5V
	PTP_PTP0 = 1;
	// PP0 output
	DDRP_DDRP0 = 1; 
	
	// PS3, PS2 serial port pins
	MODRR0_SCI1RR = 1; 
	
	// LED1 PS4
	//APPLICATON_CONTROL_LED1 = 0;
	// PS4 output
	//DDRT_DDRT0 = 1; 
	
	// LED2 PS5
	//FAULT_REPORT_LED2 = 0;
	// PS5 output
	//DDRS_DDRS1 = 1;  	
}

/*****************************************************************************
*
* Function: void InitPMF(void)
*
* Description: PMF module configuration
*
*****************************************************************************/
void InitPMF(void)
{
	// Edge aligned PWM
	PMFCFG0_EDGEA = 1;
	PMFCFG0_EDGEB = 1;
	PMFCFG0_EDGEC = 1;

	// PWM generator A generates reload event
	PMFCFG2_REV0 = 1;   
	PMFCFG2_REV1 = 0;
    
	// Low MOSFETs ON while SW control (Unipolar PWM)
	PMFOUTB = 0x2A;		
	
	// Writing to value register zero also writes to value registers one to five
	PMFCFG3_VLMODE = 0x01;	

	// Reload every PWM cycle
	PMFFQCA = 0;              
	
	// PWM clock = core clock / PWM_PRSCA = 25MHz / PWM_PRSCA 
	PMFFQCA_PRSCA = PWM_PRSCA;
	// Load modulo timer A
	PMFMODA = PWM_MODULO;
	// Load dead time timer A
	PMFDTMA = PWM_DEADTIME;

	// Mask all PWM outputs
	PMFCFG2 |= 0x3f;
	// All outputs in Software mode
	PMFOUTC_OUTCTL = 0x3f;	
	
	// PMF external global load OK signal controls reload of double buffered registers
	PMFENCA_LDOKA = 1;	
	
	 // PWM restart at commutation event
	PMFENCA_RSTRTA = 1;   
	
	// PWM generator A outputs enabled 
	PMFENCA_PWMENA = 1;

	// Buffered PMFOUTC, PMFOUTB, SWAPx and MSKx change on commutation event
	PMFCFG1_ENCE = 1;	        

	// 0 = Local LDOKA controls buffered registers / 1 = external Load OK controls buffered registers
	PMFENCA_GLDOKA = 1;         
}

/*****************************************************************************
*
* Function: void InitGDU(void)
*
* Description: GDU module configuration
*
*****************************************************************************/
void InitGDU(void)
{
	// Enable Charge pump
	GDUE_GCPE = 1; 

	// Enable Current Sense Amplifier 0
	GDUE_GCSE0 = 1; 
	
	// Clear error flags
	GDUF = 0xFF;	
	
	// Set coil current limit to maximum 750mA
	GDUBCL = 0x0F;
	
	// Set boost frequency ~ 1MHz  
	GDUCLK1_GBOCD = 0b01100;
			
	// Set duty cycle to 75%
	GDUCLK1_GBODC = 0b11;
		
	// Enable boost
	GDUE_GBOE = 1;

	// Charge pump clock = fbus / 32 
	GDUCLK2_GCPCD = 2;  

	// Blanking time ~13us
	GDUCTR = 0x13;	
	
	// GDU High level set to 26V
	GDUCTR_GHHDLVL = 1;

	// Desaturation level 1.35V
	GDUDSLVL = 0x77;

	// Enable pre-driver
	GDUE_GFDE = 1; 

	// Clear desaturation Error Flags
	GDUDSE = 0x77;	
}

/*****************************************************************************
*
* Function: void InitSCI(void)
*
* Description: SCI module configuration
*
*****************************************************************************/
void InitSCI(void)
{
	SCI1BD = SCI_BAUDRATE;
	SCI1CR2_TE = 1;
	SCI1CR2_RE = 1;
}

/*****************************************************************************
*
* Function: void InitINT(void)
*
* Description: Interrupt priorities configuration
*
*****************************************************************************/
void InitINT(void)
{
	// set ADC 0 done ISR (0x184) priority to 6
	// 0x184 / 4 = 0x61 => reg. 0x60, offset 1
	INT_CFADDR = 0x60;
	INT_CFDATA1 = 6;
	
	// set ADC 0 error ISR (0x18C) priority to 2
	// 0x18C / 4 = 0x63 => reg. 0x60, offset 3
	INT_CFADDR = 0x60;
	INT_CFDATA3 = 2;
	
	// TIM ch1 input capture ISR (0x1C8) priority set to 5
	// 0x1C8 / 4 = 0x72 => reg. value = 0x70 + offset 2
	INT_CFADDR = 0x70;
	INT_CFDATA2 = 5;
	
	// TIM ch3 output compare ISR (0x1C0) priority set to 4
	// 0x1C0 / 4 = 0x70 => reg. value = 0x70 + offset 0
	INT_CFADDR = 0x70;
	INT_CFDATA0 = 4;
}

/*****************************************************************************
*
* Function: void UpdateDutyCycle(void)
*
* Description: - Update of duty cycle to PMF
*              - Update of trigger delays
*
*****************************************************************************/
void UpdateDutyCycle(void)
{
	tU16 delay1;

	// Load PMF duty cycle
	PMFVAL0 = dutyCycle;

	// DC Bus current sampling point at middle of duty cycle
	// Calibration due to GDU signal delay
	// Divide by 2, because number of calculated ticks is related to PWM Clock = 25MHz
	// PTU is running on 12.5MHz, so it is 2 times slower
	delay1 = (PWM_DEADTIME + ((dutyCycle - PWM_DEADTIME) >> 1) + ADC_SAMPLE_TIME_CALIBRATION) >> 1;
	
	// Load TG0 list 0 and list 1
	PTUTriggerEventList[0][0][0] = delay1;
	PTUTriggerEventList[0][1][0] = delay1;

	// Clear flag
	PTUC_PTULDOK = 1;
}

/******************************************************************************
*
* Function: void AppInit(void)
*
* Description: BLDC application initialization function
*
*******************************************************************************/
void AppInit(void)
{	
	// Load 0% duty cycle
	dutyCycle = 0;
	// Select commutation sector (all transistors OFF)
	cmtSector = 6;
		
    // Select pattern
    PMFCFG2 = 0x40 + MaskVal[cmtSector];
    PMFOUTC_OUTCTL = OutCtl[cmtSector];	  
	// TIM ch0 output toggle on compare to force commutation
	TIM0TCTL2_OL0 = 1;  
	TIM0TCTL2_OM0 = 0;
	// Force commutation (apply PMF MASK/Control configuration)
	TIM0CFORC_FOC0 = 1; 
	// Disable TIM ch0 output compare action to do not generate 
	// force commutation (async_event), when TIM counter TCNT reaches value 0
	TIM0TCTL2_OL0 = 0;  
	TIM0TCTL2_OM0 = 0;
    
    //Load duty cycle
    PMFVAL0 = dutyCycle;
    // Switch list when next reload event (apply PWM duty cycle)
    PTUC_PTULDOK = 1; 
	
	appState = APP_STOP;
}

/*******************************************************************************
*
* Function: void AppStop(void)
*
* Description: BLDC application Stop state function
*
*******************************************************************************/
void AppStop(void)
{    
	uint8_t i;
	
	// Turn OFF LED1
	APPLICATON_CONTROL_LED1 = OFF;
	
	// Load PI controllers parameters
    speedPIPrms.f16UpperLimit = FRAC16(RUN_PI_UPPER_LIMIT);
    speedPIPrms.f16LowerLimit = FRAC16(RUN_PI_LOWER_LIMIT);
    speedPIPrms.f16CC1sc = FRAC16(RUN_PI_SPEED_PARAM_UP_CC1);
    speedPIPrms.f16CC2sc = 0;
    speedPIPrms.u16NShift = 0;
    speedPIPrms.f32Acc = (((tFrac32) dutyCycle <<15)/(tFrac32) PWM_MODULO) << 16;
    speedPIPrms.f16InErrK1 = 0;

    currentPIPrms.f16UpperLimit = FRAC16(RUN_PI_UPPER_LIMIT);
    currentPIPrms.f16LowerLimit = FRAC16(RUN_PI_LOWER_LIMIT);
    currentPIPrms.f16CC1sc = FRAC16(RUN_PI_CURRENT_PARAM_CC1);
    currentPIPrms.f16CC2sc = 0;
    currentPIPrms.u16NShift = 0;
    currentPIPrms.f32Acc = (((tFrac32) dutyCycle <<15)/(tFrac32) PWM_MODULO) << 16;
    currentPIPrms.f16InErrK1 = 0;
    
	// Run ?
	if(appControl.bit.StopRun == 1)
    {        
		// Reset counter
		hallEdgePerod1msCounter = 0;
		
		// Read Hall pattern
    	hallPattern = 0x07 & (PTIT >> 1);
        
        // Check Hall pattern
        if ((hallPattern == 0) | (hallPattern == 7))
        {
            // Hall pattern fault
            faultStatus.bit.HallPaternError = 1;
        }
        else
        {            	
        	// Select commutation sector
        	cmtSector = BLDCPatternBasedOnHall[rotDir][hallPattern];
        	// Select pattern
        	PMFCFG2 = 0x40 + MaskVal[cmtSector];
        	PMFOUTC_OUTCTL = OutCtl[cmtSector];
        	// TIM ch0 output toggle on compare to force commutation
        	TIM0TCTL2_OL0 = 1;  
        	TIM0TCTL2_OM0 = 0;
        	// Force commutation (apply PMF MASK/Control configuration)
        	TIM0CFORC_FOC0 = 1; 
        	// Disable TIM ch0 output compare action to do not generate 
        	// force commutation (async_event), when TIM counter TCNT reaches value 0
        	TIM0TCTL2_OL0 = 0;  
        	TIM0TCTL2_OM0 = 0;
        	
        	// TIM ch1 interrupt enable (enable commutation function execution)
        	// Clear flag
        	TIM0TFLG1 = TIM0TFLG1_C1F_MASK;
        	TIM0TIE_C1I = 1; 
        	
        	//Load duty cycle
        	PMFVAL0 = START_DUTY_CYCLE;
        	 // Switch list when next reload event (apply PWM duty cycle)
        	PTUC_PTULDOK = 1; 
        	
        	// Turn ON LED1
        	APPLICATON_CONTROL_LED1 = ON;
        	
        	// Reset counter
        	motorStallCounter = 0;
      	
        	appState = APP_RUN;
        }
    }
}

/*******************************************************************************
*
* Function: void AppRun(void)
*
* Description:  BLDC application Run state function
*
*******************************************************************************/
void AppRun(void)
{   
	if(appControl.bit.StopRun == 0)
	{ 
        appState = APP_INIT;
    }
}

/*******************************************************************************
*
* Function: void AppFault(void)
*
* Description: BLDC application Fault state function
*
*******************************************************************************/
void AppFault(void)
{    
	// Stop motor
	
	// Load 0% duty cycle
	dutyCycle = 0;
	// Select commutation sector (all transistors OFF)
	cmtSector = 6;
		
    // Select pattern
    PMFCFG2 = 0x40 + MaskVal[cmtSector];
    PMFOUTC_OUTCTL = OutCtl[cmtSector];	  
	// TIM ch0 output toggle on compare to force commutation
	TIM0TCTL2_OL0 = 1;  
	TIM0TCTL2_OM0 = 0;
	// Force commutation (apply PMF MASK/Control configuration)
	TIM0CFORC_FOC0 = 1; 
	// Disable TIM ch0 output compare action to do not generate 
	// force commutation (async_event), when TIM counter TCNT reaches value 0
	TIM0TCTL2_OL0 = 0;  
	TIM0TCTL2_OM0 = 0;
    
    //Load duty cycle
    PMFVAL0 = dutyCycle;
    // Switch list when next reload event (apply PWM duty cycle)
    PTUC_PTULDOK = 1; 
	
	// Turn ON LED2
	FAULT_REPORT_LED2 = ON;
	
	// Application turn OFF
	appControl.bit.StopRun = OFF;
	
	// Turn OFF LED1
	APPLICATON_CONTROL_LED1 = OFF;
	
	// Clear faults?
	if(appControl.bit.ClearFault == 1)
	{
		// Turn OFF LED2
		FAULT_REPORT_LED2 = OFF;
		
		// Clear label
		appControl.bit.ClearFault = 0;
		
		// Clear driver status byte
		faultStatus.byte = 0;
		
		appState = APP_INIT;
	}
}

/*******************************************************************************
*
* Function: State machine alignment function
*
* Description:  Not used
*
*******************************************************************************/
static void AppAlignment(void)
{
}

/*******************************************************************************
*
* Function: State machine start function
*
* Description:  Not used
*
*******************************************************************************/
static void AppStart(void)
{
}

/*****************************************************************************
*
* Function: void ADC0done_ISR(void)
*
* Description: ADC0 Conversion Done Interrupt Service Routine
*
*****************************************************************************/
__declspec(interrupt) void ADC0done_ISR(void)
{
	// LEFT justified result ADC data
	DCBusCurrent = ADC0ResultList[0] - DCBusCurrentOffset;
	DCBusVoltage = ADC0ResultList[1];	
	
    // Load torque array. 
	// DCBusCurrent shift 3times to ensure, that 6 * hallEdgeTorque[hallPattern] < 65535 
	motorHallEdgePeriodCurrent[hallPattern] = DCBusCurrent >> 3;
	
	// Clear flag
	ADC0CONIF = 1;      
}

/*****************************************************************************
*
* Function: void ADC0error_ISR(void)
*
* Description: ADC0 Error Interrupt Service Routine
*
*****************************************************************************/
__declspec(interrupt) void ADC0error_ISR(void)
{
	uint8_t tmpAdcEIF;

	// Load error data
	tmpAdcEIF = ADC0EIF;

	// Load Ok Error
	if (tmpAdcEIF & ADC0EIF_LDOK_EIF_MASK)        
	{  
		AdcErrorLDOK++;
		ADC0EIF = ADC0EIF_LDOK_EIF_MASK;
	}
	
	// Restart Request Error
	if (tmpAdcEIF & ADC0EIF_RSTAR_EIF_MASK) 
	{       
		AdcErrorRSTAR++;
		ADC0EIF = ADC0EIF_RSTAR_EIF_MASK;
	}
	
	// Trigger Error => Soft Reset
	if (tmpAdcEIF & ADC0EIF_TRIG_EIF_MASK) 
	{       
		AdcErrorTRIG++;
		ADC0CTL_0_ADC_SR = 1;
	}
	
	// End Of List Error => Soft Reset
	if (tmpAdcEIF & ADC0EIF_EOL_EIF_MASK) 
	{       
		AdcErrorEOL++;
		ADC0CTL_0_ADC_SR = 1;
	}
	
	 // Command Value Error => Soft Reset
	if (tmpAdcEIF & ADC0EIF_CMD_EIF_MASK) 
	{      
     	AdcErrorCMD++;
     	ADC0CTL_0_ADC_SR = 1;
	}
	
	// Illegal Access Error => Soft Reset
	if (tmpAdcEIF & ADC0EIF_IA_EIF_MASK) 
	{       
		AdcErrorIA++;
		ADC0CTL_0_ADC_SR = 1;
	}
}

/*****************************************************************************
*
* Function: void TIMch1_ISR(void)
*
* Description: Commutation routine. It takes 11us.
*              
*****************************************************************************/
__declspec(interrupt) void TIMch1_ISR(void)
{	
	// Load TIM counter value
	timerValue = TIM0TC1; 
	
	// Read Hall pattern
	hallPattern = 0x07 & (PTIT >> 1);
	
	// Control loop
	if (appState == APP_RUN)
	{
		// Select commutation sector
		cmtSector = BLDCPatternBasedOnHall[rotDir][hallPattern];
		// Select pattern
		PMFCFG2 = 0x40 + MaskVal[cmtSector];
		PMFOUTC_OUTCTL = OutCtl[cmtSector];	
	
		// Now 6.5 us from interrupt processing start
		// TIM ch0 output toggle on compare to force commutation
		TIM0TCTL2_OL0 = 1;  
		TIM0TCTL2_OM0 = 0;
		// Force commutation (apply PMF MASK/Control configuration)
		TIM0CFORC_FOC0 = 1; 
		// Disable TIM ch0 output compare action to do not generate 
		// force commutation (async_event), when TIM counter TCNT reaches value 0
		TIM0TCTL2_OL0 = 0;  
		TIM0TCTL2_OM0 = 0;
	}
	
    // Calculate period between two hall edges
    timerValueLim = timerValue - timerLastValue;
    // Limit the value to do not exceed 16bit value
    if (timerValueLim > 10900)
    {
        timerValueLim = 10900;
        //driveStatus.B.StallCheckReq = 1;
    }
    else
    {
        //driveStatus.B.StallCheckReq = 0;
    }
    // Save hall signal period (measured between two following edges)
    hallEdgePeriod[hallPattern] = timerValueLim;
    // Load timer value
    timerLastValue = timerValue; 
    
	// Reset motor stall counter
	motorStallCounter = 0;
	
	// Clear counter
	hallEdgePerod1msCounter = 0;
	
	// Clear flag
	TIM0TFLG1 = TIM0TFLG1_C1F_MASK;
}

/*****************************************************************************
*
* Function: void TIMchan3_ISR(void)
*
* Description: Timer channel 3 Output Compare Interrupt Service Routine
*              used to calculate speed, speed control loop and current control loop
*
*****************************************************************************/
__declspec(interrupt) void TIMch3_ISR(void)
{
	tFrac16 PIOut;
	uint8_t  i;

			
	// Load TIM period
	TIM0TC3 = TIM0TC3 + TIMER_1MS;
	
	// Increment 1ms free running counter 
	counter1ms++;
	
	// Check if DC bus voltage is within limits
	if(DCBusVoltage < DC_BUS_UNDERVOLTAGE)
	{
		// Set motor stall label
		faultStatus.bit.DCBusUnderVoltage = 1;
		
		
		appState = APP_FAULT;
	}
	
	// Check if DC bus voltage is within limits
	if(DCBusVoltage > DC_BUS_OVERVOLTAGE)
	{
		// Set motor stall label
		faultStatus.bit.DCBusOverVoltage = 1;
		
		
		appState = APP_FAULT;
	}
	
	// Control loop
	if (appState == APP_RUN)
	{		
		// Increment motor stall counter
		motorStallCounter++;
		
		// Check motor stall 
		if (motorStallCounter > MOTOR_STALL_1MS_COUNT)
		{			
			// Motor stall detected			
			// Set motor stall label
			faultStatus.bit.MotorStall = 1;
			
			
			appState = APP_FAULT;
		}
			
		// Calculate period between Hall signal edges
    	period6ZC = hallEdgePeriod[1] + hallEdgePeriod[2] + hallEdgePeriod[3] + \
           	    	hallEdgePeriod[4] + hallEdgePeriod[5] + hallEdgePeriod[6];
    	actualSpeed = SPEED_CALC_NUMERATOR / period6ZC;
    	
    	// To limit VHD voltage rise during motor speed-down, change PI regulator gain
    	if(speedErr < SPEED_ERROR_STEP_MAX)
    	{
    		   speedPIPrms.f16CC1sc = FRAC16(RUN_PI_SPEED_PARAM_DOWN_CC1);
    	}
    	else
    	{
    		speedPIPrms.f16CC1sc = FRAC16(RUN_PI_SPEED_PARAM_UP_CC1);
    	}
    	
		// Maximal speed limit reached?
		if (requiredSpeed > MAX_SPEED)
		{
			requiredSpeed = MAX_SPEED;
		}
		
		// Minimal speed limit reached?
		if (requiredSpeed < MIN_SPEED)
		{
			requiredSpeed = MIN_SPEED;
		}
    	
    	// Calculate speed error
    	speedErr = requiredSpeed - (tFrac16) actualSpeed;
    	speedPIOut = GFLIB_ControllerPIrAW(speedErr, &speedPIPrms, F16);
	    	
        // BLDC motor phase current measurement, if PWM duty cycle > DC_THRESHOLD to avoid incorrect values in case of small duty cycle.
        if(dutyCycle > PWM_DUTYCYCLE_MIN)
        {
            // BLDC motor phase current calculation. 
        	// motorHallEdgePeriodCurrent members shifted >> 3. Thats is why motorCurrent6ZC do not exceed value 65535
            motorCurrent6ZC = motorHallEdgePeriodCurrent[1] + motorHallEdgePeriodCurrent[2] + motorHallEdgePeriodCurrent[3] + \
            		          motorHallEdgePeriodCurrent[4] + motorHallEdgePeriodCurrent[5] + motorHallEdgePeriodCurrent[6];
        }
        else
        {
            /* Ignore current measurement if duty cycle is too small to measure
               the DC-Bus current properly */
        	motorCurrent6ZC = 0;
        }
	        
        // Divide by 6 (number of commutations) and shift left << 3 to scale motorCurrent6ZC back to <-1,1)
        motorCurrent = ((MLIB_Mul(motorCurrent6ZC, MOTOR_CURRENT_CALC_NUMERATOR, F16)) << 3);
        	        
        // Proceed only in case of PWM duty cycle higher than duty cycle minimal value 
    	if(dutyCycle > PWM_DUTYCYCLE_MIN)
    	{
    		// Check last motor phase current sample difference to calculated average value
    		motorCurrentDifference = (motorHallEdgePeriodCurrent[hallPattern] << 3) - motorCurrent;
    		// Calculate motor current difference from motor current average value 
    		motorAverageCurrentDifference = MLIB_Mul(motorCurrent, MOTOR_PHASE_CURRENT_DIFFERENCE, F16);
    		// Possible motor stall? 
    		if (motorCurrentDifference > motorAverageCurrentDifference)
    		{
    			// Use actual motor current sample, because of motor stall possibility
    			motorCurrent = motorHallEdgePeriodCurrent[hallPattern] << 3;
    		}
    	}
	    	
    	// Keep motor current positive values only.
    	// If motorCurrentError overflows, the currentPIOut is set to minimum value. The PWM duty cycle is forced to minimal value (controlled by 
    	// motor current controller). Motor speed changes rapidly to lower values, not controlled by speed loop. 
    	if (motorCurrent < 0)
    	{
    		motorCurrent = 0;
    	}
	    
    	// Filter motor current
    	motorCurrentFiltered = GDFLIB_FilterIIR1_F16(motorCurrent, &f16motorCurrentIIR1);
    	
    	// Keep motorCurrentError value from 0 to 32767 (0x7FFF).
        motorCurrentError = motorCurrentLimit - motorCurrent;
        currentPIOut = GFLIB_ControllerPIrAW(motorCurrentError, &currentPIPrms, F16);
          
        if (currentPIOut >= speedPIOut) 
        {
        	currentPIPrms.f32Acc = ((tFrac32) speedPIOut) << 16;
        	currentPIPrms.f16InErrK1 = 0;
        	PIOut = speedPIOut;
        } 
        else 
        {
        	speedPIPrms.f32Acc = ((tFrac32) currentPIOut) << 16;
        	speedPIPrms.f16InErrK1 = 0;
        	PIOut = currentPIOut;
        }

        // Calculate duty cycle
    	dutyCycle = MLIB_Mul(PIOut, PWM_MODULO, F16);  
    	// Apply duty cycle
    	UpdateDutyCycle();
    	
    	// Clear counter
    	hallEdgePerod1msCounter = 0;
	}
	else
	{
		// Increment Hall edge period counter, when application is OFF
		hallEdgePerod1msCounter++;
		
		// Motor is still running? Maximal hall period not reached?
		if (hallEdgePerod1msCounter <= HALL_EDGE_PERIOD_MAX)
		{
	    	// Calculate period between Hall signal edges
	    	period6ZC = hallEdgePeriod[1] + hallEdgePeriod[2] + hallEdgePeriod[3] + \
	       	    		hallEdgePeriod[4] + hallEdgePeriod[5] + hallEdgePeriod[6];
	            
	    	// Measurable speed ?
	    	if (period6ZC < 65400)
	    	{
	    		// Calculate motor actual speed
	    		actualSpeed = SPEED_CALC_NUMERATOR / period6ZC;
	    	}
	    	else
	    	{
	    		// Actual speed
	    		actualSpeed = 0;
	    	} 
	    	
		
	    	// BLDC motor phase current calculation. 
	    	// motorHallEdgePeriodCurrent members shifted >> 3. Thats is why motorCurrent6ZC do not exceed value 65535
	    	motorCurrent6ZC = motorHallEdgePeriodCurrent[1] + motorHallEdgePeriodCurrent[2] + motorHallEdgePeriodCurrent[3] + \
	    					  motorHallEdgePeriodCurrent[4] + motorHallEdgePeriodCurrent[5] + motorHallEdgePeriodCurrent[6];
	        
	    	// Divide by 6 (number of commutations) and shift left << 3 to scale motorCurrent6ZC back to <-1,1)
	    	motorCurrent = ((MLIB_Mul(motorCurrent6ZC, MOTOR_CURRENT_CALC_NUMERATOR, F16)) << 3);
		
	    	// Filter motor current
	    	motorCurrentFiltered = GDFLIB_FilterIIR1_F16(motorCurrent, &f16motorCurrentIIR1);
		}
		else
		{			
	    	// TIM ch1 interrupt disable (disable commutation function execution)
	    	TIM0TIE_C1I = 0;
	    	
	    	// Actual speed
	    	actualSpeed = 0;
	    	
	        // Load hall period counters
	        timerValue = 0; 
	        timerLastValue = 0;
	        
	    	// Reset motor stall counter
	        motorStallCounter = 0;
	    	
	        // Load Hall period array with maximum period (actual lowest speed) to allow control loop start-up motor
	        for (i = 0; i < 7; i++)
	        {
	        	hallEdgePeriod[i] = 10900;
	        }
	        
	        // Set motor phase current array members to 0Amps
	        for (i = 0; i < 7; i++)
	        {
	        	motorHallEdgePeriodCurrent[i] = 0;
	        }
	        
	     	// Reset motor current
	     	motorCurrent = 0;
	        motorCurrentFiltered = 0;
	        
	        // Calibrate ADC
	        DCBusCurrentOffset = ADC0ResultList[0];
	        
	    	// Load rotation direction
	    	rotDir = appControl.bit.RotDirection; 
			
	    	// Set counter
	    	hallEdgePerod1msCounter = HALL_EDGE_PERIOD_MAX;
		}
	}

	// Clear flag
	TIM0TFLG1 = TIM0TFLG1_C3F_MASK;
}

/*****************************************************************************
*
* Function: void GetKeyboard(void)
*
* Description: Read control signals
*              
*****************************************************************************/
void GetKeyboard(void)
{	
	// SW1, SW2 speed control switches
	if (switchMotorSpeedCtrl == ON)
	{
		// SW1 - motor speed-up
		if(SW1_UP == 0)
		{
			// Application turn ON?
			if (appControl.bit.StopRun == OFF)
			{
				// Application turn ON
				appControl.bit.StopRun = ON;
				
				// Set initial speed
				requiredSpeed = INITIAL_SPEED;
			}
			else
			{
				requiredSpeed = requiredSpeed + SPEED_STEP;
				
				// Maximal speed limit reached?
				if (requiredSpeed > MAX_SPEED)
				{
					requiredSpeed = MAX_SPEED;
				}
			
			}
			
			// Load next period 
			nextSwCtrlPeriod = counter1ms + CONTER_PERIOD;
			
			// Clear label
			switchMotorSpeedCtrl = OFF;
		}
		
		// SW2 - motor speed-down
		if(SW2_DOWN == 0)
		{
			requiredSpeed = requiredSpeed - SPEED_STEP;
			
			// Minimal speed limit reached?
			if (requiredSpeed < MIN_SPEED)
			{
				// Application turn OFF
				appControl.bit.StopRun = OFF;
				
				// Set initial speed
				requiredSpeed = 0;
			}
			
			// Load next period 
			nextSwCtrlPeriod = counter1ms + CONTER_PERIOD;
			
			// Clear label
			switchMotorSpeedCtrl = OFF;
		}
	}
	else
	{
		if (counter1ms >= nextSwCtrlPeriod)
		{
			// Set label
			switchMotorSpeedCtrl = ON;
		}
		
	}
}

/*****************************************************************************
*
* Function: void main(void)
*
* Description: Main routine
*
*****************************************************************************/
void main(void) 
{
	// Feed the dog
	__RESET_WATCHDOG();	
		
	InitCPMU();
	InitPIM();
	InitTIM();
	InitPMF();
	InitPTU();
	InitADC();
	InitSCI();
	InitGDU();
	InitINT();

	FMSTR_Init();
	
	EnableInterrupts;
	
	// Clear driver control byte
	appControl.byte = 0;
	faultStatus.byte = 0;
	
	// Init rotation direction clockwise
	rotDir = CW;
	
	// Reset 1ms counter
	counter1ms = 0;
	
	// Set label
	switchMotorSpeedCtrl = ON;
	
	// Load motor current limit
	motorCurrentLimit = MOTOR_CURRENT_MAX;
	
	// Set initial speed
	requiredSpeed = INITIAL_SPEED;
	
	// Load filter coefficients
	// 1st order butterworth low pass filter, ft = 10Hz, sampling period 1ms 	
	f16motorCurrentIIR1.trFiltCoeff.f16B0 = FRAC16(0.0038086);
	f16motorCurrentIIR1.trFiltCoeff.f16B1 = FRAC16(0.0038086);
	f16motorCurrentIIR1.trFiltCoeff.f16A1 = FRAC16(-0.11738);
	
	// Filter init
	GDFLIB_FilterIIR1Init_F16(&f16motorCurrentIIR1);
	
    // Init state machine
    appState = APP_INIT;

	// Loop forever
	while(1) 
	{				
		// Feed the dog
		__RESET_WATCHDOG();	
		
		// Read control signals
		GetKeyboard();
		
		// Call BLDC application state machine function
        AppStateMachine[appState]();

        // FreeMaster     
        FMSTR_Poll();
	}
} 
