/** ###################################################################
**     Filename    : ProcessorExpert.c
**     Project     : ProcessorExpert
**     Processor   : MC56F84789VLL
**     Version     : Driver 01.14
**     Compiler    : Metrowerks DSP C Compiler
**     Date/Time   : 2012-03-09, 16:14, # CodeGen: 0
**     Abstract    :
**         Main module.
**         This module contains user's application code.
**     Settings    :
**     Contents    :
**         No public methods
**
** ###################################################################*/
/* MODULE ProcessorExpert */


/* Including needed modules to compile this module/procedure */
#include "Cpu.h"
#include "Events.h"
#include "FMSTR1.h"
#include "Inhr1.h"
#include "eFPWM1.h"
#include "ADC1.h"
#include "CMP1.h"
#include "DA1.h"
#include "TI1.h"
#include "MC1.h"
#include "MC2.h"
#include "MFR1.h"
#include "EInt1.h"
#include "EInt2.h"
#include "Cap1.h"
#include "Cap2.h"
#include "Cap3.h"
/* Including shared modules, which are used for whole project */
#include "PE_Types.h"
#include "PE_Error.h"
#include "PE_Const.h"
#include "IO_Map.h"
/* Including special module, which is use for BLDC motor control */
#include "globals.h"

/******************************************************************************
* BLDC Commutation Tables
******************************************************************************/
/* PWM MASK Table*/
const unsigned int commutationTableMask[8] = 
{ /* 000,  001,  010,    011,   100,  101,   110,    111    */
  0x0770,0x0110,0x0440,0x0220,0x0220,0x0440,0x0110,0x0770
};

/* PWM SW Control Channel Table */
const unsigned int commutationTableSWC[8] = 
{
  0x0000, 0x0080, 0x0008, 0x0008, 0x0800, 0x0080, 0x0800,0x0000
};

typedef struct
{
	Frac16	speedCmdInc;
	Frac16	speedCmdMin;
	Frac16	speedCmdMax;
	Word8	countDir;
} SPEED_CMD_PROCESS_T;

/******************************************************************************
* Function headers
******************************************************************************/
static void InitializeADC(void);
static Frac16 SpeedCommandProcessing(Frac16 speedCmd, SPEED_CMD_PROCESS_T *speedCmdStat);

/******************************************************************************
* Global variables
******************************************************************************/
APPCONTROL_T appControl;
UWord16 directionOfRotation;
UWord16 captureHall1, captureHall2, captureHall3;
UWord16 hs_period;
UWord16 lowspeed_cntr;
Frac16 motor_speed;
Frac16 motor_speed_cmd;
Frac16 motor_speed_ramp;
Frac16 pwm_duty_cycle;
Frac16 i_dc_bus_filt;
Word16 i_sample_delay;
Frac16 i_dc_bus_limit = MOTOR_CURRENT_LIMT_F16;
mc_sPIparams1_limitSc speedCtrlParams = SPEED_CTRL_PARAMS_INIT;
mc_sPIparams1_limitSc currentCtrlParams = CURRENT_CTRL_PARAMS_INIT;


/******************************************************************************
* Local variables
******************************************************************************/
static Word16 voltage_scale;
static Word16 current_scale;
static Word16 speed_scale;
static Frac16 v_dc_bus_filt;
static Frac16 i_dc_bus;
static Frac32 i_dc_bus_filt_buffer;
static UWord16 hallSensor;
static SPEED_CMD_PROCESS_T speedCmdStatus = SPEED_CMD_PROCESS_INIT;


/******************************************************************************
* Local functions
******************************************************************************/
/*****************************************************************************
*
* Function: void InitializeADC(void)
*
* Description:
*         Function initializes ADC dc-bus current zero offset
*					It starts triggered ADC mode
*     
* Range Issues: None
* Special Issues: None
*****************************************************************************/
void InitializeADC(void)
{
	Word16 i_dc_bus_offset;
	Word32 adc_sample_buff = 0;
	Word16 i;

	/* wait for ADC to power up */
	while (PESL(ADC12, ADC_GET_POWER_STATUS, ADC_CONVERTER_0 | ADC_CONVERTER_1));
		
	/* clear ADC status */
	PESL(ADC12, ADC_CLEAR_STATUS_EOSI, NULL);
	/* Start ADC conversion */
	PESL(ADC12, ADC_STOP, ADC_OFF);
	
	/* Calibrate zero current offset */
	for(i=0;i<32;i++)
	{
		while(!(PESL(ADC12, ADC_GET_STATUS_EOSI, NULL))); 
		adc_sample_buff += (Word32)PESL(ADC12, ADC_READ_SAMPLE, 0);
	}

	i_dc_bus_offset = (Word16)L_shrtNs(adc_sample_buff,5);
	PESL(ADC12, ADC_WRITE_OFFSET0,(UWord16) i_dc_bus_offset);
	
	i_sample_delay = I_SAMPLE_DELAY;

	/* clear ADC status */
	PESL(ADC12, ADC_CLEAR_STATUS_EOSI, NULL);
	/* enable ADC Conversion Complete interrupt */	 
 	PESL(ADC12, ADC_INT_ENABLE, ADC_END_OF_SCAN);
}
/*****************************************************************************
*
* Function: Frac16 SpeedCommandProcessing(Frac16 speedCmd, SPEED_CMD_PROCESS_T *speedCmdStat)
*
* Description:
*   Function increment a required speed command. It is called when a push button
*   pressed
*     
* Returns: Incremented speed command value by speedCmdInc
*   
* Global Data: None
*						
* Arguments:
*   Frac16 speedCmd - actual value of speed command
*
* Range Issues: None
* Special Issues: None
*****************************************************************************/
Frac16 SpeedCommandProcessing(Frac16 speedCmd, SPEED_CMD_PROCESS_T *speedCmdStat)
{
	Frac16 speedCmdTemp;

	speedCmdTemp = speedCmd;

	speedCmdTemp += ((Word16)(speedCmdStat->countDir) * (Word16)(speedCmdStat->speedCmdInc));

	if (speedCmdStat->countDir == 1)
	{
		/* saturate maximum speed command */
		if (speedCmdTemp >= (speedCmdStat->speedCmdMax))
		{
			speedCmdTemp = speedCmdStat->speedCmdMax;
		}
	}
	else
	{
		/* saturate minimum speed command */
		if (speedCmdTemp < (-speedCmdStat->speedCmdMax))
		{
			speedCmdTemp = -speedCmdStat->speedCmdMax;
		}
	}
	
	if ((abs_s(speedCmdTemp)<speedCmdStat->speedCmdMin)) 
		speedCmdTemp = 0;
	
	return(speedCmdTemp);
}

/*****************************************************************************
*
* Function: void ComutateBLDC(unsigned int direction)
*
* Description:
*   The function decodes Hall Sensors signals and provide commutation
*     
* Returns: None
*   
* Global Data: hallSensor   - decoded stated of hall sensors
*
* Arguments:
*   unsigned int direction  - required direction of rotation
*
* Range Issues: None
* Special Issues: None
*****************************************************************************/
#pragma interrupt called 
void ComutateBLDC(unsigned int direction)
{
	register unsigned int tempHS;
	register unsigned int result;
	asm
	{
		move.w  ArchIO.gpioc.rawdata,tempHS
		clr.w   result
		bftsth  0x0008,tempHS
		rol.w   result
		bftsth  0x2000,tempHS
		rol.w   result
		bftsth  0x0040,tempHS
		rol.w   result
		tst.w   direction
		beq     clockwise
		bfchg   0x0007,result
	clockwise:
		move.w  result, hallSensor       
	}
	ArchIO.pwma.mask = commutationTableMask[hallSensor];
	ArchIO.pwma.dtsrcsel = commutationTableSWC[hallSensor];
	PESL(PWMA, PWM_SM0_FORCE_OUT_EVENT, NULL);
}

/***************************************************************************//*!
*
* Function:  void FaultControl(void)
*
* Description: This function detects following fault states
* The function provides following tasks:
*   - detection of Over-current, Over-voltage and Under-voltage faults
*
* Returns: None
*
****************************************************************************/
static void FaultControl(void)
{
	/* dc-bus overcurrent fault detection */
	if(!(ioctl(PWMA,PWM_READ_FAULTCH0_STATUS_REG,NULL) & PWM_FSTS0_FFPIN0))
    {
        appControl.B.faultpending = 0;
    }
    
    /* dc-bus undervoltage fault detection */
    if(v_dc_bus_filt < FRAC16(UNDERVOLTAGE/SCALE_VOLTAGE))
    {
    	appControl.B.lv_fault = 1;
    	appControl.B.faultpending = 1;
        appControl.B.fault = 1;
    }
    else
    {
       	appControl.B.faultpending = 0;
   	}

    /* dc-bus overvoltage fault detection */
    if(v_dc_bus_filt > FRAC16(OVERVOLTAGE/SCALE_VOLTAGE))
    {
        appControl.B.ov_fault = 1;
        appControl.B.faultpending = 1;
        appControl.B.fault = 1;
    }
    else
  	{
       	appControl.B.faultpending = 0;
  	}
    
    /* Clear fault if no fault is pending */
    if(appControl.B.faultclr == 1 && appControl.B.faultpending == 0)
    {
    	appControl.B.faultclr = 0;
    	appControl.B.fault = 0;
    	appControl.B.ov_fault = 0;
    	appControl.B.lv_fault = 0;
    	appControl.B.oi_fault = 0;
		PESL(PWMA, PWM_CLEAR_FAULTCH0_FLAG, PWM_FAULT0);
		PESL(PWMA, PWM_FAULTCH0_INTERRUPT, PWM_FAULT0_ENABLE);
    }
    else
    {
    	appControl.B.faultclr = 0;    	    	
    }
    
    /* Disable PWM outputs and set motor_speed_cmd = 0 if fault condition exists */
    if(appControl.B.fault == 1)
    {
    	/* Disable PWM outputs */
		setReg16(PWMA_OUTEN,0x0); /* Disable PWMA and PWMB for SM0,SM1,SM2) */
		motor_speed_cmd = 0;
		motor_speed_ramp = 0;
		speedCtrlParams.IntegralPortionK_1 = 0;
    }
  	
}

/*****************************************************************************
*
* Function: void ADC_EndofScanISR (void)
*
* Description:
*  Interrupt service routine called at ADC End of Scan interrupt. It is called
*  every second PWM cycle (every 100us). 
*		- read ADC result sfor dc-bus voltage and current
*       - if the motor speed is under low speed the routine perform BLDC motor commutation. 
*       - it executes FreeMASTER recorder routine
*     
* Returns: None
*   
* Global Data: i_dc_bus   - dc-bus current adc conversion result
*              v_dc_bus_filt - dc-bus voltage result (moving average filter 4 samples)
*              i_dc_bus_filt - filtered dc-bus current (moving average filter 16 samples)
*              i_dc_bus_filt_buffer - dc-bus current moving average filter buffer
*              lowspeed_cntr - counter detectning low speed area
*              appControl - application control and status word
*
* Arguments: None
*   
*
* Range Issues: None
* Special Issues: None
*****************************************************************************/
#pragma interrupt saveall
void ADC_EndofScanISR(void)
{
	/* Read ADC conversion results */
	/* i dc-bus reading */
	i_dc_bus = (Word16)PESL(ADC12, ADC_READ_SAMPLE, 0);

	/* v dc-bus reading and filtering moving 4 sample average */
	v_dc_bus_filt += mult(sub((Frac16)PESL(ADC12, ADC_READ_SAMPLE, 1),v_dc_bus_filt), FRAC16(0.25));

	/* Apply moving average filter on dc-bus current */
	i_dc_bus_filt_buffer += i_dc_bus;
	i_dc_bus_filt = (Word16)L_shrtNs(i_dc_bus_filt_buffer,2);
	i_dc_bus_filt_buffer -= i_dc_bus_filt;

	if(++lowspeed_cntr>LOW_SPEED_THRESHOLD)
	{
		lowspeed_cntr--;
					
		/* Force motor commutation */		
		ComutateBLDC(directionOfRotation);
		/* set low speed flag */
		appControl.B.lowspeed = 1;
	}
		
	/* Execute FreeMASTER recorder */
	FMSTR1_Recorder();

	PESL(ADC12, ADC_CLEAR_STATUS_EOSI, NULL);
}

/*****************************************************************************
*
* Function: void PWM_Fault0_ISR (void)
*
* Description:
*  Interrupt service routine called when Fault0 is active. 
*		- set system fault
*       - disable PWM output 
*       - clear motor speed command
*     
* Returns: None
*   
* Global Data: appControl - application control and status word
*              motor_speed_cmd - motor command speed value
*              motor_speed_ramp - motor ramp speed value
*
* Arguments: None
*   
*
* Range Issues: None
* Special Issues: None
*****************************************************************************/
#pragma interrupt saveall
void PWM_Fault0_ISR(void)
{
	appControl.B.fault = 1;
	appControl.B.faultpending = 1;
	appControl.B.oi_fault = 1;
		
	/* Disable PWM outputs */
	setReg16(PWMA_OUTEN,0x0); /* Disable PWMA and PWMB for SM0,SM1,SM2) */
	motor_speed_cmd = 0;
	motor_speed_ramp = 0;
	PESL(PWMA, PWM_FAULTCH0_INTERRUPT, PWM_FAULT0_DISABLE);	
}

/*****************************************************************************
*
* Function: void main(void)
*
* Description:
*       application main function entered after DSC start-up. It executes
*		Processor Expert initialization of DSC core and peripherals.
*       It executes push button handling and FreeMASTER polling function
*		in endless loop on application background
*					
*     
* Range Issues: None
* Special Issues: None
*****************************************************************************/
void main(void)
{
  /* Write your local variable definition here */

  /*** Processor Expert internal initialization. DON'T REMOVE THIS CODE!!! ***/
  PE_low_level_init();
  
  /* XBARA_SEL8: SEL16=1 */
  clrSetReg16Bits(XBARA_SEL6, 0x24U, 0x1BU);
  
  /*** End of Processor Expert internal initialization.                    ***/
  /* initialize scales for FreeMASTER Display */
  voltage_scale = (Word16)(10 * SCALE_VOLTAGE);
  current_scale = (Word16)(200 * SCALE_CURR);
  speed_scale = (Word16)SCALE_MOTOR_RPM;
	
  directionOfRotation = 0;
  ComutateBLDC( directionOfRotation);
	
  /* Initialize ADC and calculate zero current offset */
  InitializeADC();
	
  /* wait for supply volatge to stabilize */
  Cpu_Delay100US(1000);

  PESL(PWMA, PWM_CLEAR_FAULTCH0_FLAG, PWM_FAULT0);
  PESL(PWMA, PWM_FAULTCH0_INTERRUPT, PWM_FAULT0_ENABLE);
  /* Write your code here */
  
  /* Background Endless Loop */
  for(;;) 
  {
	  /* periodically call fault control logic in app. main */
	  FaultControl();
	  		
	  if(!EInt1_GetVal()) EInt1_Enable();
	  if(!EInt2_GetVal()) EInt2_Enable();

	  
	  if(appControl.B.button_down == 1)
	  {
		  appControl.B.button_down = 0;
	      speedCmdStatus.countDir = - 1;
	      /* Execute Speed Command Processing */
	      motor_speed_cmd = SpeedCommandProcessing(motor_speed_cmd,&speedCmdStatus);
	  }

	  if(appControl.B.button_up == 1)
	  {
		  appControl.B.button_up = 0;
	      speedCmdStatus.countDir =  1;
	      /* Execute Speed Command Processing */
	      motor_speed_cmd = SpeedCommandProcessing(motor_speed_cmd,&speedCmdStatus);
	  }

	  if(motor_speed_ramp < 0)
		  directionOfRotation = 1;
	  else
		  directionOfRotation = 0;
	  			
	  if(motor_speed_cmd!=0 && appControl.B.fault == 0) /* RUN State */
		  /* Enable PWM outputs */
		  setReg16(PWMA_OUTEN,0x770); /* enable PWMA and PWMB for SM0,SM1,SM2) */
	  else
		  /* Disable PWM outputs */
		  setReg16(PWMA_OUTEN,0x0); /* Disable PWMA and PWMB for SM0,SM1,SM2) */
	  				
	  /* periodically call FreeMASTER polling function in app. main */
	  FMSTR1_Poll();
  }
}

/* END ProcessorExpert */
/*
** ###################################################################
**
**     This file was created by Processor Expert 5.3 [05.01]
**     for the Freescale 56800 series of microcontrollers.
**
** ###################################################################
*/
