/** ###################################################################
**     Filename  : freemaster_demo_48pin.C
**     Project   : freemaster_demo_48pin
**     Processor : MC56F8006_48_LQFP
**     Version   : Driver 01.13
**     Compiler  : Metrowerks DSP C Compiler
**     Date/Time : 14.11.2008, 10:28
**     Abstract  :
**         Main module.
**         This module contains user's application code.
**     Settings  :
**     Contents  :
**         No public methods
**
**     (c) Copyright UNIS, a.s. 1997-2008
**     UNIS, a.s.
**     Jundrovska 33
**     624 00 Brno
**     Czech Republic
**     http      : www.processorexpert.com
**     mail      : info@processorexpert.com
** ###################################################################*/


/* Including needed modules to compile this module/procedure */
#include "Cpu.h"
#include "Events.h"
#include "SCI1.h"
#include "PWM1.h"
#include "PDB1.h"
#include "ADC2.h"
#include "PB.h"
#include "QTMR0.h"
#include "PIT1.h"
#include "MC1.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"

/* include FreeMASTER Serial Driver functions */
#include "freemaster.h"

/******************************************************************************
* Constants and macros
******************************************************************************/
#define SCALE_VOLTAGE 9.0	/* Volts */
#define SCALE_CURR 1.0 /* Amps */
#define MOTOR_RPM_MAX 8600.0 /* RPM */
#define MOTOR_COMMUTATIONS_PER_REV 24 /* Motor commutations per one motor revolution */
#define MOTOR_CURRENT_LIMT 1.0 /* Amps */

#define IP_BUS_CLK_MHZ 32.0	/* MHz*/
#define TMR0_PRSC	128 /* TMR0 Prescaler */
#define TMR0_CLK ((IP_BUS_CLK_MHZ * 1E6)/TMR0_PRSC) /* TMR0 Clock Frequency*/
/* Dividend constant used to evaluate motor revolutions from timer tics
   captured by Hall sensor edges */
#define SPEED_DIV_CONST (Word16)((TMR0_CLK * 60 * 3)/(MOTOR_RPM_MAX * MOTOR_COMMUTATIONS_PER_REV))

#define SPEED_CMD_MIN FRAC16(500/MOTOR_RPM_MAX)
#define SPEED_CMD_MAX FRAC16(MOTOR_RPM_MAX/MOTOR_RPM_MAX)
#define SPEED_CMD_INC FRAC16((10*MOTOR_RPM_MAX)/(100*MOTOR_RPM_MAX))
#define SPEED_CMD_PROCESS_INIT {SPEED_CMD_INC, SPEED_CMD_MIN, SPEED_CMD_MAX, 1}

#define LOW_SPEED_THRESHOLD 900 /* 900*50us ~ 45ms */

/* PWM Zero Voltage Offset */
#define	PWM_OFFSET 0x3FFF

/* Motor Current Limit Constant in Frac16 */
#define MOTOR_CURRENT_LIMT_F16 FRAC16(MOTOR_CURRENT_LIMT/(2*SCALE_CURR))

/* Definition of Control Bits of Application  Status and Control Word (appControl) */
#define	PWM_ON	 BIT_0		/**< PWM Output Enable On/Off */
#define	LOWSPEED  BIT_1		/**< Low Speed Region */
#define	FAULTCLR	 BIT_7		/**< Fault Clear */
#define	OV_FAULT	 BIT_13		/**< Over-Voltage Fault */
#define	OI_FAULT	 BIT_14		/**< Over-Current Fault */
#define	FAULT		 BIT_15		/**< Application Fault */

/************************************************************************/
/* Commutation table for BLDC Motor                                     */
/************************************************************************/
#define MASK_SWAP_VECTOR_30DEG \
                   PWM_CCTRL_ENHA    | /* Set ENHA bit     */ \
                   PWM_CCTRL_nBX     | /* Set nBx bit      */ \
                   PWM_CCTRL_VLMODE0 | /* Set VLMODE to 01 */ \
                   PWM_CCTRL_MSK2    | /* Mask Phase B     */ \
                   PWM_CCTRL_MSK3    | /* Mask Phase B     */ \
                   PWM_CCTRL_SWP45     /* Swapt Phase C    */

#define MASK_SWAP_VECTOR_90DEG \
                   PWM_CCTRL_ENHA    | /* Set ENHA bit     */ \
                   PWM_CCTRL_nBX     | /* Set nBx bit      */ \
                   PWM_CCTRL_VLMODE0 | /* Set VLMODE to 01 */ \
						 PWM_CCTRL_MSK0    | /* Mask Phase A     */ \
						 PWM_CCTRL_MSK1    | /* Mask Phase A     */ \
						 PWM_CCTRL_SWP45     /* Swapt Phase C    */

#define MASK_SWAP_VECTOR_150DEG \
                   PWM_CCTRL_ENHA    | /* Set ENHA bit     */ \
                   PWM_CCTRL_nBX     | /* Set nBx bit      */ \
                   PWM_CCTRL_VLMODE0 | /* Set VLMODE to 01 */ \
						 PWM_CCTRL_MSK4    | /* Mask Phase C     */ \
						 PWM_CCTRL_MSK5    | /* Mask Phase C     */ \
						 PWM_CCTRL_SWP01     /* Swapt Phase A    */

#define MASK_SWAP_VECTOR_210DEG \
                   PWM_CCTRL_ENHA    | /* Set ENHA bit     */ \
                   PWM_CCTRL_nBX     | /* Set nBx bit      */ \
                   PWM_CCTRL_VLMODE0 | /* Set VLMODE to 01 */ \
						 PWM_CCTRL_MSK2    | /* Mask Phase B     */ \
						 PWM_CCTRL_MSK3    | /* Mask Phase B     */ \
						 PWM_CCTRL_SWP01     /* Swapt Phase A    */

#define MASK_SWAP_VECTOR_270DEG \
                   PWM_CCTRL_ENHA    | /* Set ENHA bit     */ \
                   PWM_CCTRL_nBX     | /* Set nBx bit      */ \
                   PWM_CCTRL_VLMODE0 | /* Set VLMODE to 01 */ \
						 PWM_CCTRL_MSK0    | /* Mask Phase A     */ \
						 PWM_CCTRL_MSK1    | /* Mask Phase A     */ \
						 PWM_CCTRL_SWP23     /* Swapt Phase B    */

#define MASK_SWAP_VECTOR_330DEG \
                   PWM_CCTRL_ENHA    | /* Set ENHA bit     */ \
                   PWM_CCTRL_nBX     | /* Set nBx bit      */ \
                   PWM_CCTRL_VLMODE0 | /* Set VLMODE to 01 */ \
						 PWM_CCTRL_MSK4    | /* Mask Phase C     */ \
						 PWM_CCTRL_MSK5    | /* Mask Phase C     */ \
						 PWM_CCTRL_SWP23     /* Swapt Phase B    */

/*!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!*/
/* Modify table according to Hall sensor signal coding of the specific motor */
/*!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!*/
/* Hall sensor coding table */
const UWord16 bldcHallSensorTable[] = {
		0,
		MASK_SWAP_VECTOR_270DEG,
		MASK_SWAP_VECTOR_30DEG,
		MASK_SWAP_VECTOR_330DEG,
		MASK_SWAP_VECTOR_150DEG,
		MASK_SWAP_VECTOR_210DEG,
		MASK_SWAP_VECTOR_90DEG,
		0 };


/* initialize PI speed controller parameters */
#define SPEED_CTRL_PARAMS_INIT \
{\
	16384, /* ProportionalGain = 16384 * 2^0*/\
	16384, /* IntegralGain = 16384 * 2^-5 */\
	0,  /* IntegralPortionK_1 */\
	0x7FFF, /* PositivePILimit 0x7FFF*/\
	0x0, /* NegativePILimit */\
	0, /* 0 ProportionalGainScale */\
	5 /* 5 IntegralGainScale */\
}

/* initialize PI current controller parameters */
#define CURRENT_CTRL_PARAMS_INIT\
{\
	16384, /* ProportionalGain = 16384 * 2^-0 = 16*/\
	16384, /* IntegralGain = 16384 * 2^-5 */\
	0x7FFFFFFF,  /* IntegralPortionK_1 */\
	0x7FFF, /* PositivePILimit 0x7FFF*/\
	0x0, /* NegativePILimit */\
	0, /* 0 ProportionalGainScale */\
	5 /* 5 IntegralGainScale */\
}

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


/******************************************************************************
* Function headers
******************************************************************************/
void InitializeADC(void);
void InitializeHS(void);
Frac16 SpeedCommandProcessing(Frac16 speedCmd, SPEED_CMD_PROCESS_T *speedCmdStat);
void ComutateBLDC(void);
bool IsIrq2ButtonSet(void);

/******************************************************************************
* Local variables
******************************************************************************/
static Word16 voltage_scale;
static Word16 current_scale;
static Word16 speed_scale;
static Frac16 v_dc_bus_filt;
static Word16 i_dc_bus_offset;
static Frac16 i_dc_bus;
static Frac16 i_dc_bus_filt;
static Frac32 i_dc_bus_filt_buffer;
static Frac16 i_dc_bus_limit = MOTOR_CURRENT_LIMT_F16;
static UWord16 pwm_mask_swap;
static UWord16 hs_port;
static UWord16 appControl;
static UWord16 capture_old;
static Word16 capture_input_pin;
static UWord16 period_buff[3];
static UWord8 buff_i;
static Word16 hs_period;
static Frac16 motor_speed;
static Frac16 motor_speed_cmd;
static Frac16 pwm_duty_cycle;
static UWord16 stand_still_counter;
static SPEED_CMD_PROCESS_T speedCmdStatus = SPEED_CMD_PROCESS_INIT;
static mc_sPIparams1_limitSc speedCtrlParams = SPEED_CTRL_PARAMS_INIT;
static mc_sPIparams1_limitSc currentCtrlParams = CURRENT_CTRL_PARAMS_INIT;
volatile bool bIrq2ButtonFlag = false;


/******************************************************************************
* Local functions
******************************************************************************/
void InitializeADC(void)
{
	Word32 adc_sample_buff = 0;
	Word16 i;

	for(i=0;i<32;i++)
	{
		while(!(PESL(ADC0, ADC_CONV_COMPLETEB, NULL)));
		adc_sample_buff += (Word32)ArchIO.adc0.adcrb;
	}

	 i_dc_bus_offset = (Word16)L_shrtNs(adc_sample_buff,5);

}
/******************************************************************************/
#pragma interrupt called
void InitializeHS(void)
{
	UWord16 portB_data;

  	/* initialize hall sensor status */
   /* Read Hall Sensor code from GPIOB2, GPIOB4 and GPIOB5) */
  	portB_data = PESL(GPIOB, GPIO_READ_RAW_DATA, NULL);
	hs_port = ((portB_data>>2)&BIT_0)+((portB_data>>3)&(BIT_1|BIT_2));
  switch(hs_port)
  {
  case 1:
  case 6:
  default:
  	  	capture_input_pin = 0;
  		break;
  case 2:
  case 5:
  	  	capture_input_pin = 1;
  		break;
  case 3:
  case 4:
  	  	capture_input_pin = 2;
  		break;
  }
}
/******************************************************************************/
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 < speedCmd))
		{
			speedCmdTemp = speedCmdStat->speedCmdMax;
			speedCmdStat->countDir = (Word8)((speedCmdStat->countDir) * (-1));
		}
	}
	else
	{
		/* saturate minimum speed speed command */
		if ((speedCmdTemp < (speedCmdStat->speedCmdMin)) || (speedCmdTemp > speedCmd))
		{
			speedCmdTemp = 0;
			speedCmdStat->countDir = (Word8)((speedCmdStat->countDir) * (-1));
		}
	}
	return(speedCmdTemp);
}
/******************************************************************************/
#pragma interrupt called
void ComutateBLDC(void)
{
	periphMemWrite(pwm_mask_swap, &PWM_CCTRL);
}
/******************************************************************************/
bool IsIrq2ButtonSet(void)
{
   if(bIrq2ButtonFlag == true)
   {
      bIrq2ButtonFlag = false;

      return true;
   }

   return false;
}
/******************************************************************************/
#pragma interrupt
void ISR_Irq2Button(void)
{

   bIrq2ButtonFlag = true;

   /* Clear GPIO interrupt */
   PESL(GPIOB, GPIO_CLEAR_INT_PENDING, BIT_3);
}
/******************************************************************************/
#pragma interrupt saveall
void ISR_SpeedController(void)
{

	if(appControl&LOWSPEED)
	{
		/* if low speed detected motor speed = 0 */
		motor_speed = 0;
	}
	else
	{
		/* if speed > minimum speed calculate speed */
		if(hs_period <= SPEED_DIV_CONST) hs_period=SPEED_DIV_CONST;
		motor_speed = div_s(SPEED_DIV_CONST, hs_period);
	}

	/* Execute DC-Bus Current PI Controller */
	/* Current controller sets output limit of the speed current controller */
   speedCtrlParams.PositivePILimit = MC1_controllerPItype1_asmSc(i_dc_bus_limit, i_dc_bus_filt, &currentCtrlParams);

	/* Execute Speed PI Controller */
	pwm_duty_cycle = MC1_controllerPItype1_asmSc(motor_speed_cmd, motor_speed, &speedCtrlParams);

		/* Apply required PWM duty-cycle */
	PESL(PWM, PWM_UPDATE_VALUE_REG_0, (pwm_duty_cycle>>1) + PWM_OFFSET);

	PESL(PIT, PIT_CLEAR_FLAG, NULL);
}

/******************************************************************************/
#pragma interrupt
void ISR_TimerCapture(void)
{
	UWord16 capture_actual;
	UWord16 capture_period;
	UWord16 portB_data;

   capture_actual = PESL(TMR0, QT_READ_CAPTURE_REG, NULL);
   capture_period = capture_actual - capture_old;
   capture_old = capture_actual;

   if(capture_period>10922) capture_period = 10922; /* 10922 = 32767/3 */

   period_buff[buff_i++] = capture_period;
   if(buff_i>2) buff_i = 0;

   hs_period = (Word16)period_buff[0] + (Word16)period_buff[1] + (Word16)period_buff[2];

   /* Read Hall Sensor code from GPIOB2, GPIOB4 and GPIOB5) */
  	portB_data = PESL(GPIOB, GPIO_READ_RAW_DATA, NULL);
	hs_port = ((portB_data>>2)&BIT_0)+((portB_data>>3)&(BIT_1|BIT_2));

	/* Decode rotor position angle */
	pwm_mask_swap = bldcHallSensorTable[hs_port];

	/* Commutate the motor */
	ComutateBLDC();

   if (--capture_input_pin < 0) capture_input_pin = 2;

   /* clear stand-still counter */
   stand_still_counter = 0;

	/* clear low speed bit */
	appControl &= ~LOWSPEED;

   PESL(TMR0, QT_SET_SECONDARY_SOURCE, capture_input_pin);

   /* Clear edge interrupt */
   PESL(TMR0, QT_CLEAR_FLAG, QT_INPUT_EDGE_FLAG);

}
/******************************************************************************/
#pragma interrupt saveall
void  ISR_ADCA_Complete(void)
{

	/* v dc-bus reading and filtering moving 4 sample average */
	/* v_dc_bus filter type 1 - moving average 10 cycles/7 instructions */
	v_dc_bus_filt += mult(sub((Frac16)PESL(ADC0, ADC_READ_DATA_RSLTA_REG, NULL),v_dc_bus_filt), FRAC16(0.25));
	i_dc_bus = (Word16)ArchIO.adc0.adcrb - i_dc_bus_offset;

	/* 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,4);
	i_dc_bus_filt_buffer -= i_dc_bus_filt;

	if(++stand_still_counter>LOW_SPEED_THRESHOLD)
	{
		stand_still_counter--;
				/* Initiali hall senor status */
		InitializeHS();
  		/* Decode rotor position angle */
  		pwm_mask_swap = bldcHallSensorTable[hs_port];
  		/* force motor commutation */
  		ComutateBLDC();

		/* set low speed flag */
		appControl |= LOWSPEED;

  		/* fill speed buffer */
  		period_buff[buff_i++] = 10922;
   	if(buff_i>2) buff_i = 0;

	}
		/* Call FreeMASTER Recorder */
	FMSTR_Recorder();
}
#pragma interrupt off

/***************************************************************************//*!
*
* @brief  Application main
*
* @return Nothing
*
* @remarks This is application main.
* The main function is called after CW start-up routine is finished.
* The function provides following tasks:
*   - peripheral initialization
*   - application variables initialization
*   - application background control loop running in end-less loop
*
****************************************************************************/
void main(void)
{
  Word16 i;

  /* Write your local variable definition here */

  /*** Processor Expert internal initialization. DON'T REMOVE THIS CODE!!! ***/
  PE_low_level_init();
  /*** End of Processor Expert internal initialization.                    ***/

  /* initialize FreeMASTER */
  FMSTR_Init();

	/* initialize scales for FreeMASTER Display */
  voltage_scale = (Word16)(10 * SCALE_VOLTAGE);
  current_scale = (Word16)(200 * SCALE_CURR);
  speed_scale = (Word16)MOTOR_RPM_MAX;

  /* initialize speed buffer */
  for(i=0;i<3;i++) period_buff[i] = 10922;

	/* Initialize hall sesnor status */
	InitializeHS();

  /* Decode rotor position angle */
  pwm_mask_swap = bldcHallSensorTable[hs_port];
  /* force motor commutation */
  ComutateBLDC();

  /* Initialize ADC offset */
  InitializeADC();

  /* Enable CPU Interrupts */
  Cpu_EnableInt();

	/* Background Endless Loop */
  while(1)
  {

    if(IsIrq2ButtonSet() == true)
    {
		/* Execute Speed Command Processing */
		motor_speed_cmd = SpeedCommandProcessing(motor_speed_cmd,&speedCmdStatus);
    }

	if(motor_speed_cmd>0) /* RUN State */
	{
		ioctl(PWM, PWM_OUTPUT_PAD, PWM_ENABLE);
	}
	else
	{
		/* Switch off PWM outputs and reset control variables */
		ioctl(PWM, PWM_OUTPUT_PAD, PWM_DISABLE);
	}

  	/* Call FreeMASTER polling function */
  	FMSTR_Poll();
  }

}

/* END bldc_hall_demo_48pin */
/*
** ###################################################################
**
**     This file was created by UNIS Processor Expert 2.99 [04.17]
**     for the Freescale 56800 series of microcontrollers.
**
** ###################################################################
*/
