/******************************************************************************
*
* (c) Copyright 2012, Freescale Semiconductor Inc.
*
***************************************************************************//*!
*
* @file     main.c
*
* @author   Milan Brejl
*
* @date     25-Jan-2012
*
* @brief    Sample application demonstrating the functionality of the PMSM 
*           Vector Control Drive implemented on the eTPU.
*
******************************************************************************/
#include "MPC5554.h"
#include "freemaster.h"
#include "fs_gpio.h"
#include "etpu_app_pmsmrsvc.h"  /* eTPU PMSMRSVC App API */
#include "etpu_pmsmvc.h"        /* eTPU PMSMVC API */
#include "etpu_sc.h"            /* eTPU SC API */
#include "etpu_asac.h"          /* eTPU ASAC API */
#include "etpu_pwmmac.h"        /* eTPU PWMMAC API */
#include "pmsmrsvc_etpu_gct.h"  /* eTPU configuration created by eTPU GCT */
#include "MC33937_routines.h"   /* Routines for MC33937 predriver */
#include "state_machine.h"      /* Application state machine types and defines */


/******************************************************************************
* PMSM Configuration
******************************************************************************/
#define CALIB_DURATION			50 /* in [ms] */
#define ALIGN_DURATION			250 /* in [ms] */
#define ALIGN_VOLTAGE 			0x080000	/* 24-bit fract */

#define GPIO_UP             FS_GPIO_EMIOS0
#define GPIO_DOWN           FS_GPIO_EMIOS1
#define GPIO_RUN_STOP       FS_GPIO_EMIOS2

#define HW_SWITCH_ON        0
#define HW_SWITCH_OFF       1

#define SPEED_MAX_RPM       3500
#define SPEED_INC_RPM       50

/******************************************************************************
* Local function prototypes
******************************************************************************/
void GPIO_Init(void);
void FMPLL_Init(void);
void EQADC_EDMA_Init(void);
void EQADC_EDMA_Start(void);
void ESCI_A_Init(void);
void stateReset(void);
void stateFault(void);
void stateInit(void);
void stateReady(void);
void stateCalib(void);
void stateAlign(void);
void stateRun(void);

/******************************************************************************
* Global variables
******************************************************************************/
/* PMSMRSVC eTPU application */
pmsmrsvc_instance_t pmsmrsvc_instance;
pmsmrsvc_data_t     pmsmrsvc_data;
int32_t speedRequiredRPM = 0;

/* HW and SW switches and buttons */
static uint8_t switchRunStopSW, switchRunStopSWLast;
static uint8_t switchRunStopHW, switchRunStopHWLast;
static uint8_t switchUpHW, switchUpHWLast;
static uint8_t switchDownHW, switchDownHWLast;
static uint8_t switchControlType = FS_ETPU_APP_PMSMRSVC_SPEED_LOOP_CLOSED;
static uint8_t switchDiagnosticsRSLV, switchDiagnosticsRSLVLast;
static uint8_t switchClearFaultsRSLV = TURNED_OFF;
static uint8_t switchCalibrateRSLV = TURNED_OFF;

/* Resolver diagnostics */
uint32_t faultRSLV;

/* State machine */
static AppStates       state;
static AppEvents       event;
static AppFaultStatus  faultID;
static uint16_t        calibCntr;
static uint16_t        alignCntr;


static PFCN_VOID_VOID state_table[13][7] =
{   /* Actual state ->       'Reset'       'Init'           'Fault'         'Ready'         'Calib'         'Align'         'Run'*/
    /* e_reset          */ { stateReset,    stateReset,     stateReset,     stateReset,     stateReset,     stateReset,     stateReset},
    /* e_reset_done     */ { stateInit,     stateFault,     stateFault,     stateFault,     stateFault,     stateFault,     stateFault},
    /* e_fault          */ { stateFault,    stateFault,     stateFault,     stateFault,     stateFault,     stateFault,     stateFault},
    /* e_fault_clear    */ { stateFault,    stateFault,     stateInit,      stateFault,     stateFault,     stateFault,     stateFault},
    /* e_init_done      */ { stateFault,    stateReady,     stateFault,     stateFault,     stateFault,     stateFault,     stateFault},
    /* e_ready          */ { stateFault,    stateFault,     stateFault,     stateReady,     stateFault,     stateFault,     stateFault},
    /* e_app_on         */ { stateFault,    stateFault,     stateFault,     stateCalib,     stateFault,     stateFault,     stateFault},
    /* e_calib          */ { stateFault,    stateFault,     stateFault,     stateFault,     stateCalib,     stateFault,     stateFault},
    /* e_calib_done     */ { stateFault,    stateFault,     stateFault,     stateFault,     stateAlign,     stateFault,     stateFault},
    /* e_align          */ { stateFault,    stateFault,     stateFault,     stateFault,     stateFault,     stateAlign,     stateFault},
    /* e_align_done     */ { stateFault,    stateFault,     stateFault,     stateFault,     stateFault,     stateRun,       stateFault},
    /* e_run            */ { stateFault,    stateFault,     stateFault,     stateFault,     stateFault,     stateFault,     stateRun},
    /* e_app_off        */ { stateFault,    stateFault,     stateFault,     stateReady,     stateInit,      stateInit,      stateInit}
};

/* DMA transfers */
uint32_t * p_ASAC_result_queue;
uint32_t * p_ASAC_command_queue;

/* eTPU functions DATA RAM basis for FreeMASTER */
vuint32_t * PWMMAC_pba;
vuint32_t * PMSMVC_pba;
vuint32_t * PMSMVC_PID_D_pba;
vuint32_t * PMSMVC_PID_Q_pba;
vuint32_t * SC_pba;
vuint32_t * SC_PID_pba;
vuint32_t * BC_pba;
vuint32_t * ASAC_pba;
vuint32_t * RSLV_pba;
extern uint32_t fs_etpu_data_ram_start;

/* eTPU functions Configuration Register basis for FreeMASTER */
vuint32_t * PWMMAC_cba;
vuint32_t * PWM_A_BASE_cba;
vuint32_t * PWM_A_COMPL_cba;
vuint32_t * PWM_B_BASE_cba;
vuint32_t * PWM_B_COMPL_cba;
vuint32_t * PWM_C_BASE_cba;
vuint32_t * PWM_C_COMPL_cba;
vuint32_t * PMSMVC_cba;
vuint32_t * SC_cba;
vuint32_t * BC_cba;
vuint32_t * ASAC_cba;
vuint32_t * RSLV_cba;

/* MC33937 predriver */
static  MC33937_SET_T predriver = MPC5554_BGA416_CB_DEFAULT;


/******************************************************************************
* FreeMASTER TSA tables
******************************************************************************/
/*
 * With TSA enabled, the user describes the global and static variables using 
 * so-called TSA tables. There can be any number of tables defined in 
 * the project files. Each table does have the identifier which should be
 * unique across the project. 
 *
 * Note that you can declare variables as Read-Only or Read-Write.
 * The FreeMASTER driver denies any write access to the Read-Only variables
 * when TSA_SAFETY is enabled.
 */

FMSTR_TSA_TABLE_BEGIN(fmstr_tsa_table_state_machine)
    FMSTR_TSA_RO_VAR(state, FMSTR_TSA_UINT8)
    FMSTR_TSA_RO_VAR(event, FMSTR_TSA_UINT8)
    FMSTR_TSA_RO_VAR(faultID, FMSTR_TSA_UINT32)
FMSTR_TSA_TABLE_END()

FMSTR_TSA_TABLE_BEGIN(fmstr_tsa_table_control)
    FMSTR_TSA_RW_VAR(switchRunStopSW, FMSTR_TSA_UINT8)
    FMSTR_TSA_RW_VAR(speedRequiredRPM, FMSTR_TSA_UINT32)
    FMSTR_TSA_RW_VAR(switchControlType, FMSTR_TSA_UINT8)
FMSTR_TSA_TABLE_END()

FMSTR_TSA_TABLE_BEGIN(fmstr_tsa_table_pmsmrsvc)
    FMSTR_TSA_RO_VAR(pmsmrsvc_data,  FMSTR_TSA_USERTYPE(pmsmrsvc_data_t))

    FMSTR_TSA_STRUCT(pmsmrsvc_data_t)
    FMSTR_TSA_MEMBER(pmsmrsvc_data_t, speed_ramp_rpm, FMSTR_TSA_UINT32)
    FMSTR_TSA_MEMBER(pmsmrsvc_data_t, speed_actual_rpm, FMSTR_TSA_UINT32)
    FMSTR_TSA_MEMBER(pmsmrsvc_data_t, torque_required_mnm, FMSTR_TSA_UINT32)
    FMSTR_TSA_MEMBER(pmsmrsvc_data_t, torque_actual_mnm, FMSTR_TSA_UINT32)
    FMSTR_TSA_MEMBER(pmsmrsvc_data_t, applied_voltage_mv, FMSTR_TSA_UINT32)
    FMSTR_TSA_MEMBER(pmsmrsvc_data_t, revolution_counter, FMSTR_TSA_UINT32)
    FMSTR_TSA_MEMBER(pmsmrsvc_data_t, direction, FMSTR_TSA_UINT8)
    FMSTR_TSA_MEMBER(pmsmrsvc_data_t, sc_saturation, FMSTR_TSA_UINT8)
    FMSTR_TSA_MEMBER(pmsmrsvc_data_t, d_saturation, FMSTR_TSA_UINT8)
    FMSTR_TSA_MEMBER(pmsmrsvc_data_t, q_saturation, FMSTR_TSA_UINT8)
FMSTR_TSA_TABLE_END()

FMSTR_TSA_TABLE_BEGIN(fmstr_tsa_table_rslv)
    FMSTR_TSA_RW_VAR(switchCalibrateRSLV, FMSTR_TSA_UINT8)
    FMSTR_TSA_RW_VAR(switchDiagnosticsRSLV, FMSTR_TSA_UINT8)
    FMSTR_TSA_RW_VAR(switchClearFaultsRSLV, FMSTR_TSA_UINT8)
    FMSTR_TSA_RO_VAR(faultRSLV, FMSTR_TSA_UINT32)
FMSTR_TSA_TABLE_END()

FMSTR_TSA_TABLE_BEGIN(fmstr_tsa_table_etpu)
    FMSTR_TSA_RO_VAR(eTPU, FMSTR_TSA_UINT32)
    FMSTR_TSA_RO_VAR(p_ASAC_result_queue, FMSTR_TSA_UINT32)
    FMSTR_TSA_RO_VAR(p_ASAC_command_queue, FMSTR_TSA_UINT32)
    FMSTR_TSA_RO_VAR(PWMMAC_pba, FMSTR_TSA_UINT32)
    FMSTR_TSA_RO_VAR(PMSMVC_pba, FMSTR_TSA_UINT32)
    FMSTR_TSA_RO_VAR(PMSMVC_PID_D_pba, FMSTR_TSA_UINT32)
    FMSTR_TSA_RO_VAR(PMSMVC_PID_Q_pba, FMSTR_TSA_UINT32)
    FMSTR_TSA_RO_VAR(SC_pba, FMSTR_TSA_UINT32)
    FMSTR_TSA_RO_VAR(SC_PID_pba, FMSTR_TSA_UINT32)
    FMSTR_TSA_RO_VAR(BC_pba, FMSTR_TSA_UINT32)
    FMSTR_TSA_RO_VAR(ASAC_pba, FMSTR_TSA_UINT32)
    FMSTR_TSA_RO_VAR(RSLV_pba, FMSTR_TSA_UINT32)
    FMSTR_TSA_RO_VAR(fs_etpu_data_ram_start, FMSTR_TSA_UINT32)
    FMSTR_TSA_RO_VAR(PWMMAC_cba, FMSTR_TSA_UINT32)
    FMSTR_TSA_RO_VAR(PWM_A_BASE_cba, FMSTR_TSA_UINT32)
    FMSTR_TSA_RO_VAR(PWM_A_COMPL_cba, FMSTR_TSA_UINT32)
    FMSTR_TSA_RO_VAR(PWM_B_BASE_cba, FMSTR_TSA_UINT32)
    FMSTR_TSA_RO_VAR(PWM_B_COMPL_cba, FMSTR_TSA_UINT32)
    FMSTR_TSA_RO_VAR(PWM_C_BASE_cba, FMSTR_TSA_UINT32)
    FMSTR_TSA_RO_VAR(PWM_C_COMPL_cba, FMSTR_TSA_UINT32)
    FMSTR_TSA_RO_VAR(PMSMVC_cba, FMSTR_TSA_UINT32)
    FMSTR_TSA_RO_VAR(SC_cba, FMSTR_TSA_UINT32)
    FMSTR_TSA_RO_VAR(BC_cba, FMSTR_TSA_UINT32)
    FMSTR_TSA_RO_VAR(ASAC_cba, FMSTR_TSA_UINT32)
    FMSTR_TSA_RO_VAR(RSLV_cba, FMSTR_TSA_UINT32)
FMSTR_TSA_TABLE_END()

/*
 * This list describes all TSA tables which should be exported to the 
 * FreeMASTER application.
 */
FMSTR_TSA_TABLE_LIST_BEGIN()
    FMSTR_TSA_TABLE(fmstr_tsa_table_state_machine)
    FMSTR_TSA_TABLE(fmstr_tsa_table_control)
    FMSTR_TSA_TABLE(fmstr_tsa_table_pmsmrsvc)
    FMSTR_TSA_TABLE(fmstr_tsa_table_rslv)
    FMSTR_TSA_TABLE(fmstr_tsa_table_etpu)
FMSTR_TSA_TABLE_LIST_END()


/******************************************************************************
* Interrupt handlers
******************************************************************************/

/***************************************************************************//*!
*
* @brief   Interrupt from SC - eTPU A channel 5, 1kHz
*
* @return  N/A
*
******************************************************************************/
void etpu_sc_isr(void)
{
  /* Read Up/Down HW push buttons */
  switchUpHWLast = switchUpHW;
  switchDownHWLast = switchDownHW;
  switchUpHW = fs_gpio_read_data(GPIO_UP);
  switchDownHW = fs_gpio_read_data(GPIO_DOWN);
  
  /* Read RUN/STOP HW switch */
  switchRunStopHWLast = switchRunStopHW;
  switchRunStopHW = fs_gpio_read_data(GPIO_RUN_STOP);
  /* HW switch changed? */
  if (switchRunStopHW ^ switchRunStopHWLast)
  {
    switchRunStopSW = (switchRunStopHW == HW_SWITCH_ON) ? TURNED_ON : TURNED_OFF;
  }
  /* SW switch changed? */
  if (switchRunStopSW ^ switchRunStopSWLast)
  {
    switchRunStopSWLast = switchRunStopSW;
    /* Application State Machine - event from the RUN/STOP switch */
    event = (switchRunStopSW == TURNED_ON) ? e_app_on : e_app_off;
  }
  
  /* Set Resolver diagnostic ON/OFF */
  if( switchDiagnosticsRSLV ^ switchDiagnosticsRSLVLast)
  {
    if(switchDiagnosticsRSLV == TURNED_ON)
    {
      fs_etpu_rslv_diagnostics_on(pmsmrsvc_instance.RSLV_channel);
    }
    else
    {
      fs_etpu_rslv_diagnostics_off(pmsmrsvc_instance.RSLV_channel);
      faultRSLV = 0;
    }
    switchDiagnosticsRSLVLast = switchDiagnosticsRSLV;
  }

  /* Read resolver faults resulting from RSLV Diagnostics*/
  if(switchDiagnosticsRSLV == TURNED_ON)
  {
    faultRSLV = fs_etpu_rslv_get_fault(pmsmrsvc_instance.RSLV_channel);
  }

  /* Clear Resolver diagnostic faults */
  if(switchClearFaultsRSLV == TURNED_ON)
  {
    fs_etpu_rslv_clear_fault(pmsmrsvc_instance.RSLV_channel);
    switchClearFaultsRSLV = TURNED_OFF;  
  }
  
  /* Read MC33937 faults */
  MC33937_read_SR(&predriver);
  faultID.R |= (uint32_t)predriver.status.sr0.R;
  if(faultID.R != 0)
  {
    event = e_fault;
  }

  /* Application State Machine */
  state_table[event][state]();
  
  /* Get internal PMSMRSVC data */
  fs_etpu_app_pmsmrsvc_get_data(&pmsmrsvc_instance, &pmsmrsvc_data);
  
  /* clear interrupt flag */
  fs_etpu_clear_chan_interrupt_flag(APP_PMSMRSVC0_SC);
}

/***************************************************************************//*!
*
* @brief   Interrupt from PWMMAC - eTPU B channel 7, 20kHz
*
* @return  N/A
*
******************************************************************************/
void etpu_pwmmac_isr(void)
{
  /* FreeMASTER Recorder sampling */
  FMSTR_Recorder();
  
  /* clear interrupt flag */
  fs_etpu_clear_chan_interrupt_flag(APP_PMSMRSVC0_PWM_MASTER);
}

/***************************************************************************//*!
*
* @brief   eTPU Global Interrupt - an error in eTPU operation
*
* @return  N/A
*
******************************************************************************/
void etpu_global_isr(void)
{
  uint32_t err;
  
  /* read the global exception flags */
  err = fs_etpu_get_global_exceptions();
  
  /* test the cause of the exception */
  if(err & (FS_ETPU_MICROCODE_GLOBAL_EX_A | FS_ETPU_MICROCODE_GLOBAL_EX_B))
  {
    faultID.B.eTPUErrorMicrocode = 1;
    /* use fs_etpu_get_global_error() to identify the unhandled event */
  }
  if(err & (FS_ETPU_ILLEGAL_INSTRUCTION_A | FS_ETPU_ILLEGAL_INSTRUCTION_B))
  {
    faultID.B.eTPUErrorIllegalInstr = 1;
  }
  if(err & FS_ETPU_SCM_MISC_FLAG)
  {
    faultID.B.eTPUErrorSCMMISC = 1;
  }
  if(err & (FS_ETPU_WATCHDOG_TIMEOUT_A | FS_ETPU_WATCHDOG_TIMEOUT_B))  /* eTPU2 only */
  {
    faultID.B.eTPUErrorWatchdog = 1;
  }
  if(err & FS_ETPU_SDM_READ_ERROR)  /* eTPU2 only */
  {
    faultID.B.eTPUErrorSDMRead = 1;
  }
  if(err & FS_ETPU_SCM_READ_ERROR)  /* eTPU2 only */
  {
    faultID.B.eTPUErrorSCMRead = 1;
  }
  if(err & FS_ETPU_SHARED_SUBSYS_ACC_ERR)  /* eTPU2 only */
  {
    faultID.B.eTPUErrorSubsysAcc = 1;
  }

  /* set fault event */
  if(faultID.R != 0)
  {
    event = e_fault;
  }

  /* clear interrupt flag */
  fs_etpu_clear_global_exceptions();
}

/***************************************************************************//*!
*
* @brief   Main. Initialization and background loop.
*
* @return  N/A
*
* @note    The main routine includes only initialization and start of 
*          individual modules and a background loop. All the run-time
*          application processing is handled in interrupt service routine(s).
*          The background loop is used to run FreeMASTER communation.
*
******************************************************************************/
int main(void) 
{
  state = reset;
  event = e_reset;

  /* First call of the state machine fcn pointers to enter RESET state. */
  state_table[event][state]();
	
  /* Loop forever */
  for (;;) 
  {
    /* FreeMASTER processing on background */
    FMSTR_Poll();
  }
}


/***************************************************************************//*!
*
* @brief   Init eQADC and eDMA.
*
* @return  N/A
*
******************************************************************************/
void EQADC_EDMA_Init(void)
{
  uint32_t tmp;
  struct tcd_t EDMA_tcd_rfifo_0_1 =
  {
    0,  /* source address */
    0,  /* source address modulo */
    1,  /* source transfer size */
    0,  /* destination address modulo */
    1,  /* destination transfer size */
    0,  /* signed source address offset */
    2,  /* inner (“minor”) byte count */
    0,  /* last source address adjustment */
    0,  /* destination address */
    0,  /* enable channel-to-channel linking on "minor" loop complete */
    4,  /* counter "major" iteration count */
    2,  /* signed destination address offset */
   -8,  /* last destination address adjustment, or
           scatter/gather address (if e_sg = 1) */
    0,  /* enable channel-to-channel linking on "major" loop complete */
    4,  /* beginning "major" iteration count */
    0,  /* bandwidth control */
    0,  /* link channel number on major loop complete */
    0,  /* channel done */
    0,  /* channel active */
    0,  /* enable channel-to-channel link on "major" loop complete */
    0,  /* enable scatter/gather descriptor (e_sg) */
    0,  /* disable ipd_req when done */
    0,  /* enable interrupt on major loop half complete */
    0,  /* enable interrupt on major loop completion */
    0   /* explicit channel start */
  };
  struct tcd_t EDMA_tcd_rfifo_2 =
  {
    0,  /* source address */
    0,  /* source address modulo */
    1,  /* source transfer size */
    0,  /* destination address modulo */
    1,  /* destination transfer size */
    0,  /* signed source address offset */
    2,  /* inner (“minor”) byte count */
    0,  /* last source address adjustment */
    0,  /* destination address */
    0,  /* enable channel-to-channel linking on "minor" loop complete */
    2,  /* counter "major" iteration count */
    2,  /* signed destination address offset */
   -4,  /* last destination address adjustment, or
           scatter/gather address (if e_sg = 1) */
    0,  /* enable channel-to-channel linking on "major" loop complete */
    2,  /* beginning "major" iteration count */
    0,  /* bandwidth control */
    0,  /* link channel number on major loop complete */
    0,  /* channel done */
    0,  /* channel active */
    0,  /* enable channel-to-channel link on "major" loop complete */
    0,  /* enable scatter/gather descriptor (e_sg) */
    0,  /* disable ipd_req when done */
    0,  /* enable interrupt on major loop half complete */
    0,  /* enable interrupt on major loop completion */
    0   /* explicit channel start */
  };
  
  struct tcd_t EDMA_tcd_cfifo =
  {
    0,  /* source address */
    0,  /* source address modulo */
    2,  /* source transfer size */
    0,  /* destination address modulo */
    2,  /* destination transfer size */
    4,  /* signed source address offset */
    4,  /* inner (“minor”) byte count */
  -40,  /* last source address adjustment */
    0,  /* destination address */
    0,  /* enable channel-to-channel linking on "minor" loop complete */
   10,  /* counter "major" iteration count */
    0,  /* signed destination address offset */
    0,  /* last destination address adjustment, or
           scatter/gather address (if e_sg = 1) */
    0,  /* enable channel-to-channel linking on "major" loop complete */
   10,  /* beginning "major" iteration count */
    0,  /* bandwidth control */
    0,  /* link channel number on major loop complete */
    0,  /* channel done */
    0,  /* channel active */
    0,  /* enable channel-to-channel link on "major" loop complete */
    0,  /* enable scatter/gather descriptor (e_sg) */
    0,  /* disable ipd_req when done */
    0,  /* enable interrupt on major loop half complete */
    0,  /* enable interrupt on major loop completion */
    0   /* explicit channel start */
  };
  struct tcd_t EDMA_tcd_rslv =
  {
    0,  /* source address */
    0,  /* source address modulo */
    1,  /* source transfer size */
    0,  /* destination address modulo */
    1,  /* destination transfer size */
    8,  /* signed source address offset */
    4,  /* inner (“minor”) byte count */
  -16,  /* last source address adjustment */
    0,  /* destination address */
    0,  /* enable channel-to-channel linking on "minor" loop complete */
    1,  /* counter "major" iteration count */
    4,  /* signed destination address offset */
   -8,  /* last destination address adjustment, or
           scatter/gather address (if e_sg = 1) */
    0,  /* enable channel-to-channel linking on "major" loop complete */
    1,  /* beginning "major" iteration count */
    0,  /* bandwidth control */
    0,  /* link channel number on major loop complete */
    0,  /* channel done */
    0,  /* channel active */
    0,  /* enable channel-to-channel link on "major" loop complete */
    0,  /* enable scatter/gather descriptor (e_sg) */
    0,  /* disable ipd_req when done */
    0,  /* enable interrupt on major loop half complete */
    0,  /* enable interrupt on major loop completion */
    0   /* explicit channel start */
  };

  /* Enable ADC0 and set ADC clock prescaler, via commands */
  EQADC.CFCR[0].R = 0x0200; /* invalidate CFIFO */
  EQADC.CFPR[0].R = 0x80800401 + (0 << 25); /* push ADC0_CR configuration command to CFIFO */
  EQADC.CFCR[0].R = 0x0410; /* set software triggered single scan mode and enable single scan */
  while (EQADC.FISR[0].B.EOQF != 1) {} /* wait for end-of-queue flag */  
  EQADC.FISR[0].R = ~0UL; /* clear all flags */
  /* Enable ADC1 and set ADC clock prescaler, via commands */
  EQADC.CFCR[1].R = 0x0200; /* invalidate CFIFO */
  EQADC.CFPR[1].R = 0x80800401 + (1 << 25); /* push ADC1_CR configuration command to CFIFO */
  EQADC.CFCR[1].R = 0x0410; /* set software triggered single scan mode and enable single scan */
  while (EQADC.FISR[1].B.EOQF != 1) {} /* wait for end-of-queue flag */  
  EQADC.FISR[1].R = ~0UL; /* clear all flags */

  /* eQADC RFIFO 0,1,2 and eDMA 1,3,5 */
  /* ensure RFIFOs are empty */
  while (EQADC.FISR[0].B.RFCTR > 0) tmp = EQADC.RFPR[0].R;
  while (EQADC.FISR[1].B.RFCTR > 0) tmp = EQADC.RFPR[1].R;
  while (EQADC.FISR[2].B.RFCTR > 0) tmp = EQADC.RFPR[2].R;
  /* clear all flags */
  EQADC.FISR[0].R = ~0UL;   
  EQADC.FISR[1].R = ~0UL;
  EQADC.FISR[2].R = ~0UL;
  /* write eDMA channels 1,3,5 TCD */
  EDMA.TCD[1] = EDMA_tcd_rfifo_0_1;
  EDMA.TCD[3] = EDMA_tcd_rfifo_0_1;
  EDMA.TCD[5] = EDMA_tcd_rfifo_2;
  /* set eDMA source/destination addresses */
  EDMA.TCD[1].SADDR = (uint32_t)((uint16_t *)&EQADC.RFPR[0].R + 1);
  EDMA.TCD[3].SADDR = (uint32_t)((uint16_t *)&EQADC.RFPR[1].R + 1);
  EDMA.TCD[5].SADDR = (uint32_t)((uint16_t *)&EQADC.RFPR[2].R + 1);
  EDMA.TCD[1].DADDR = (uint32_t)((uint16_t *)p_ASAC_result_queue + 0);
  EDMA.TCD[3].DADDR = (uint32_t)((uint16_t *)p_ASAC_result_queue + 4);
  EDMA.TCD[5].DADDR = (uint32_t)((uint16_t *)p_ASAC_result_queue + 8);
  /* eQADC CFIFO 2 and eDMA 4 */
  /* eDMA4 to fill eQADC CFIFO[2] on CFIFO[2] fill flag */
  EDMA.TCD[4] = EDMA_tcd_cfifo;  /* write TCD stracture to eDMA channel 4 registers */
  EDMA.TCD[4].SADDR = fs_etpu_asac_get_eQADC_cmds_addr(APP_PMSMRSVC0_ASAC);
  EDMA.TCD[4].DADDR = (uint32_t)&EQADC.CFPR[2].R;
  /* eTPU DATA RAM - move from ASAC result queue to RSLV by eDMA 0 and 2 */
  EDMA.TCD[0] = EDMA_tcd_rslv;
  EDMA.TCD[2] = EDMA_tcd_rslv;
  EDMA.TCD[0].SADDR = (uint32_t)((uint16_t *)p_ASAC_result_queue + 1);
  EDMA.TCD[2].SADDR = (uint32_t)((uint16_t *)p_ASAC_result_queue + 3);
  EDMA.TCD[0].DADDR = (uint32_t)((uint16_t *)fs_etpu_rslv_get_sincos_addr(APP_PMSMRSVC0_RSLV) + 1);
  EDMA.TCD[2].DADDR = (uint32_t)((uint16_t *)fs_etpu_rslv_get_sincos_addr(APP_PMSMRSVC0_RSLV) + 5);
  /* link eDMA 0 from eDMA 1, and eDMA 2 from eDMA 3 */
  EDMA.TCD[1].CITERE_LINK = 1;
  EDMA.TCD[1].BITERE_LINK = 1;
  EDMA.TCD[1].BITER |= (0 << 9);  /* CITER_LINKCH */
  EDMA.TCD[3].CITERE_LINK = 1;
  EDMA.TCD[3].BITERE_LINK = 1;
  EDMA.TCD[3].BITER |= (2 << 9);  /* CITER_LINKCH */

  /* RFIFO drain select eDMA and enable */
  EQADC.IDCR[0].R = 0x0003; 
  EQADC.IDCR[1].R = 0x0003;
  EQADC.IDCR[2].R = 0x0303; /* also CFIFO fill select eDMA and enable */

  /* eQADC CFIFO[2] external trigger input */
  SIU.ETISR.B.TSEL2 = 1; /* CFIFO 2 trigger = eTPU channel 29 */            
}

/***************************************************************************//*!
*
* @brief   Start eQADC and eDMA.
*
* @return  N/A
*
******************************************************************************/
void EQADC_EDMA_Start(void)
{
  /* Start eDMA (first) and eQADC (second) service */
  /* Enable eDMA channels 0, 1, 2, 3, 4, 5 */ 
  EDMA.ERQRL.R = 0x0000003f;
  /* MODE = both edge external trigger, continuous scan */
  /* This effectively starts eQADC conversions (if any) */
  EQADC.CFCR[2].R = 0x00E0;
}

/***************************************************************************//*!
*
* @brief   Init 128MHz MCU bus clock using 8MHz crystal.
*
* @return  N/A
*
******************************************************************************/
void FMPLL_Init(void)
{
  FMPLL.SYNCR.B.PREDIV = 1;          // 1: use incoming OSC 8MHz / 2 = 4 MHz
  FMPLL.SYNCR.B.MFD = 28;            // 28: VCO = 4 * (28+4) = 256 MHz
  FMPLL.SYNCR.B.RFD = 1;             // 1 - div2: fcore = VCO / (2 * 2 ) = 64MHz
  while(FMPLL.SYNSR.B.LOCK != 1){;}  // Wait for FMPLL to Lock
  FMPLL.SYNCR.B.RFD = 0;             // 0 - div1: fcore = VCO / (2 * 1 ) = 128MHz
}

/***************************************************************************//*!
*
* @brief   Init ESCI A to 115kbd @ 128MHz.
*
* @return  N/A
*
******************************************************************************/
void ESCI_A_Init(void)
{
  SIU.PCR[89].B.PA =1;               // Pin asigned to ESCI A Tx
  SIU.PCR[89].B.OBE =1;              // Open drain enable
  SIU.PCR[90].B.PA =1;               // Pin asigned to ESCI A Rx
  SIU.PCR[90].B.IBE =1;              // Input buffer enable

  ESCI_A.CR2.R = 0x2000;             // Enable ESCI and set all bits to reset value
  
  ESCI_A.CR1.B.TE = 1;               // receiver enable
  ESCI_A.CR1.B.RE = 1;               // transmitter enable
  ESCI_A.CR1.B.PT = 0;               // parity is even
  ESCI_A.CR1.B.PE = 0;               // parity control disable
  ESCI_A.CR1.B.SBR = 69;             // Baud rate = 115200 @ 128MHz
}

/***************************************************************************//*!
*
* @brief   Init device I/O pin.
*
* @return  N/A
*
******************************************************************************/
void GPIO_Init(void)
{
  const uint16_t GPIO_output_pad_config =
    FS_GPIO_IO_FUNCTION +
    FS_GPIO_MAXIMUM_SLEW_RATE + 
    FS_GPIO_OUTPUT_DRAIN_DISABLE +
    FS_GPIO_READBACK_ENABLE +
    FS_GPIO_OUTPUT_BUFFER_ENABLE;
  const uint16_t GPIO_input_pad_config =
    FS_GPIO_IO_FUNCTION +
    FS_GPIO_INPUT_BUFFER_ENABLE;

  const uint16_t eTPU_output_pad_config =
    FS_GPIO_PRIMARY_FUNCTION +
    FS_GPIO_MAXIMUM_SLEW_RATE + 
    FS_GPIO_OUTPUT_DRAIN_DISABLE +
    FS_GPIO_READBACK_ENABLE +
    FS_GPIO_OUTPUT_BUFFER_ENABLE;
  const uint16_t eTPU_input_pad_config =
    FS_GPIO_PRIMARY_FUNCTION +
    FS_GPIO_INPUT_BUFFER_ENABLE;
  
  const uint16_t DSPI_output_pad_config =
    FS_GPIO_PRIMARY_FUNCTION +
    FS_GPIO_MAXIMUM_SLEW_RATE + 
    FS_GPIO_OUTPUT_DRAIN_DISABLE +
    FS_GPIO_READBACK_ENABLE +
    FS_GPIO_OUTPUT_BUFFER_ENABLE;

  const uint16_t DSPI_input_pad_config =
    FS_GPIO_PRIMARY_FUNCTION +
    FS_GPIO_INPUT_BUFFER_ENABLE;
 
  /* Initialize RSLV output pads */
  fs_gpio_config(FS_GPIO_ETPUA0, eTPU_output_pad_config);
  /* ASAC output is internally connected to eQADC trigger,
   * hence the signal is not needed on a device pin, 
   * but it can be used to synchronize a scope */
  fs_gpio_config(FS_GPIO_ETPUA29, eTPU_output_pad_config);
  /* Initialize SC and PMSMVC output pads */
  fs_gpio_config(FS_GPIO_ETPUB5, eTPU_output_pad_config); //for debugging only
  fs_gpio_config(FS_GPIO_ETPUB6, eTPU_output_pad_config); //for debugging only
  /* Initialize PWM output pads */
  fs_gpio_config(FS_GPIO_ETPUB7, eTPU_output_pad_config); //for debugging only
  fs_gpio_config(FS_GPIO_ETPUB8, eTPU_output_pad_config);
  fs_gpio_config(FS_GPIO_ETPUB9, eTPU_output_pad_config);
  fs_gpio_config(FS_GPIO_ETPUB10, eTPU_output_pad_config);
  fs_gpio_config(FS_GPIO_ETPUB11, eTPU_output_pad_config);
  fs_gpio_config(FS_GPIO_ETPUB12, eTPU_output_pad_config);
  fs_gpio_config(FS_GPIO_ETPUB13, eTPU_output_pad_config);
  /* Initilize BC output pad */
  fs_gpio_config(FS_GPIO_ETPUB14, eTPU_output_pad_config);

  /* MC33937 DSPI */
  fs_gpio_config(FS_GPIO_EMIOS13, GPIO_output_pad_config); //Enable MC33937
  fs_gpio_config(FS_GPIO_EMIOS12, GPIO_output_pad_config); //Reset MC33937
  fs_gpio_config(FS_GPIO_SCKA, DSPI_output_pad_config);  //DSPIA MC33937
  fs_gpio_config(FS_GPIO_SINA, DSPI_input_pad_config);   //DSPIA MC33937
  fs_gpio_config(FS_GPIO_SOUTA, DSPI_output_pad_config); //DSPIA MC33937
  fs_gpio_config(FS_GPIO_PCSA0, DSPI_output_pad_config); //DSPIA MC33937
  
  /* Applicationa switches */
  fs_gpio_config(GPIO_UP, GPIO_input_pad_config);
  fs_gpio_config(GPIO_DOWN, GPIO_input_pad_config);
  fs_gpio_config(GPIO_RUN_STOP, GPIO_input_pad_config);
}


/***************************************************************************//*!
*
* @brief   FAULT state
*
* @param   void
*
* @return  none
*
******************************************************************************/
static void stateFault(void)
{
  /* Record in which state the fault occured */
  if(state != fault)
  {
    faultID.R |= (uint32_t)1 << (state + 16);
  }
  
  /* Application State Machine - state identification */
  state = fault;
  
  /* Disable generation of PWM signals */
  fs_etpu_app_pmsmrsvc_disable(&pmsmrsvc_instance);

  /* Turn RUN/STOP switch OFF */
  switchRunStopSW     = TURNED_OFF;
  switchRunStopSWLast = TURNED_OFF;

  /* Reset required speed */
  speedRequiredRPM = 0;

  /* Clear the fault flags if HW switch was turned OFF*/
  if((switchRunStopHWLast == HW_SWITCH_ON) && (switchRunStopHW == HW_SWITCH_OFF))
  {
    MC33937_clear_fault(&predriver);
    faultID.R = 0x0;
    event = e_fault_clear;
  }
}


/***************************************************************************//*!
*
* @brief   RESET state
*
* @param   void
*
* @return  none
*
******************************************************************************/
static void stateReset(void)
{
  /* Application State Machine - state identification */
	state = reset;

  /* Application State Machine variables */
	switchRunStopSW     = TURNED_OFF;
	switchRunStopSWLast = TURNED_OFF;
	faultID.R           = 0x0;

	/* Initialize pins */
	GPIO_Init();

	/* Initialize sysclk to 100 MHz */
	FMPLL_Init();

	/* Initialize ESCI 0 to 115200bd@100MHz for FreeMASTER communication */                
	ESCI_A_Init();

	/* Initialize DSPI for communication with MC33937 */
	DSPI_init();

	/* Initialize MC33937 */
	MC33937_config(&predriver);
	MC33937_enable(&predriver);
	MC33937_read_SR(&predriver);

	/* Initialize and setup eTPU */
	if(my_system_etpu_init() != 0)
	{
	  faultID.B.HwErrorETPU = 1;
	}
	p_ASAC_command_queue = (uint32_t*) fs_etpu_asac_get_eQADC_cmds_addr(APP_PMSMRSVC0_ASAC);

	/* Get eTPU functions DATA RAM addresses for FreeMASTER */
	PWMMAC_pba      = fs_etpu_data_ram(APP_PMSMRSVC0_PWM_MASTER);
	PMSMVC_pba      = fs_etpu_data_ram(APP_PMSMRSVC0_PMSMVC);
	PMSMVC_PID_D_pba= (uint32_t*)(fs_etpu_data_ram_start + fs_etpu_get_chan_local_24(APP_PMSMRSVC0_PMSMVC, FS_ETPU_PMSMVC_PPIDD_OFFSET));
	PMSMVC_PID_Q_pba= (uint32_t*)(fs_etpu_data_ram_start + fs_etpu_get_chan_local_24(APP_PMSMRSVC0_PMSMVC, FS_ETPU_PMSMVC_PPIDQ_OFFSET));
	SC_pba          = fs_etpu_data_ram(APP_PMSMRSVC0_SC);
	SC_PID_pba      = (uint32_t*)(fs_etpu_data_ram_start + fs_etpu_get_chan_local_24(APP_PMSMRSVC0_SC, FS_ETPU_SC_PPID_OFFSET));
	BC_pba          = fs_etpu_data_ram(APP_PMSMRSVC0_BC);
	ASAC_pba        = fs_etpu_data_ram(APP_PMSMRSVC0_ASAC);
	RSLV_pba        = fs_etpu_data_ram(APP_PMSMRSVC0_RSLV);
	/* Get addresses of channel configuration registers for FreeMASTER */
	PWMMAC_cba      = &eTPU->CHAN[APP_PMSMRSVC0_PWM_MASTER].CR.R;
	PWM_A_BASE_cba  = &eTPU->CHAN[APP_PMSMRSVC0_PWM_PHASEA_BASE_CHANNEL].CR.R;
	PWM_A_COMPL_cba = &eTPU->CHAN[APP_PMSMRSVC0_PWM_PHASEA_COMPL_CHANNEL].CR.R;
	PWM_B_BASE_cba  = &eTPU->CHAN[APP_PMSMRSVC0_PWM_PHASEB_BASE_CHANNEL].CR.R;
	PWM_B_COMPL_cba = &eTPU->CHAN[APP_PMSMRSVC0_PWM_PHASEB_COMPL_CHANNEL].CR.R;
	PWM_C_BASE_cba  = &eTPU->CHAN[APP_PMSMRSVC0_PWM_PHASEC_BASE_CHANNEL].CR.R;
	PWM_C_COMPL_cba = &eTPU->CHAN[APP_PMSMRSVC0_PWM_PHASEC_COMPL_CHANNEL].CR.R;
	PMSMVC_cba      = &eTPU->CHAN[APP_PMSMRSVC0_PMSMVC].CR.R;
	SC_cba          = &eTPU->CHAN[APP_PMSMRSVC0_SC].CR.R;
	BC_cba          = &eTPU->CHAN[APP_PMSMRSVC0_BC].CR.R;
	ASAC_cba        = &eTPU->CHAN[APP_PMSMRSVC0_ASAC].CR.R;
  RSLV_cba        = &eTPU->CHAN[APP_PMSMRSVC0_RSLV].CR.R;

	/* Initialize FreeMASTER */
	FMSTR_Init();

	/* Initialize eQADC and eDMA */
	EQADC_EDMA_Init();

	/* Install interrupt handlers */
	INTC_InstallINTCInterruptHandler(etpu_global_isr, 67, 5); /* eTPU global exception */
  INTC_InstallINTCInterruptHandler(etpu_pwmmac_isr, 243 + (APP_PMSMRSVC0_PWM_MASTER - 64), 4); /* PWMMAC */
  INTC_InstallINTCInterruptHandler(etpu_sc_isr, 243 + (APP_PMSMRSVC0_SC - 64), 3); /* SC */

	/* Enable interrupts */
	INTC.MCR.B.HVEN = 0;
	INTC.MCR.B.VTES = 0;
	INTC.CPR.B.PRI  = 0;
	asm("wrteei 1");

	/* Start eDMA and eQADC */
	EQADC_EDMA_Start();

	/* Start eTPU */
	my_system_etpu_start();

	/* Reset sequence has finished */
	event = e_reset_done;
}

/***************************************************************************//*!
*
* @brief   INIT state
*
* @param   void
*
* @return  none
*
******************************************************************************/
static void stateInit(void)
{
  /* Application State Machine - state identification */
  state = init;

  /* Application State Machine variables */
  switchRunStopSW     = TURNED_OFF;
  switchRunStopSWLast = TURNED_OFF;

  /* Disable PWM outputs */
  fs_etpu_app_pmsmrsvc_disable(&pmsmrsvc_instance);

  /* Reset required speed */
  speedRequiredRPM = 0;

  /* Application State Machine - ready for next state */
  event = e_init_done;
  calibCntr = CALIB_DURATION;
}

/***************************************************************************//*!
*
* @brief   READY state
*
* @param   void
*
* @return  none
*
******************************************************************************/
static void stateReady(void)
{
  /* Application State Machine - state identification */
  state = ready;
  event = e_ready;

  /* Reset required speed */
  speedRequiredRPM = 0;
}

/***************************************************************************//*!
*
* @brief   CALIBRATION state - ADC calibration state
*
* @param   void
*
* @return  none
*
******************************************************************************/
static void stateCalib(void)
{
  /* Application State Machine - state identification */
  state = calib;

  /* Reset required speed */
  speedRequiredRPM = 0;
  
  /* Application State Machine - state entry */
  if(event != e_calib)
  {
    /* Start calibration */
    if(fs_etpu_app_pmsmrsvc_calib_start(&pmsmrsvc_instance) == 0)
    {
      event = e_calib;
    }
  }

  /* Application State Machine - ready for next state? */
  if (--calibCntr <= 0)
  {
    /* Finish calibration */
    fs_etpu_app_pmsmrsvc_calib_finish(&pmsmrsvc_instance);

    /* Application State Machine - calibration finished */
    event = e_calib_done;
    alignCntr = ALIGN_DURATION;
  }
}

/***************************************************************************//*!
*
* @brief   ALIGNMENT state - motor control axes alignment
*
* @param   void
*
* @return  none
*
******************************************************************************/
static void stateAlign(void)
{
  /* Application State Machine - state identification */
  state = align;

  /* Reset required speed */
  speedRequiredRPM = 0;

  /* Application State Machine - state entry */
  if(event != e_align)
  {
    /* Start alignment */
    if(fs_etpu_app_pmsmrsvc_align_start(&pmsmrsvc_instance, ALIGN_VOLTAGE) == 0)
    {
      event = e_align;
    }
  }
    
  /* Return 0 angle from RSLV during alignment */
  fs_etpu_rslv_set_position(pmsmrsvc_instance.RSLV_channel, 0);

  /* Application State Machine - alignment done? */
  if (--alignCntr <= 0)
  {
    /* Finish alignment */
    fs_etpu_app_pmsmrsvc_align_finish(&pmsmrsvc_instance, switchControlType);

    /* Application State Machine - alignment finished */
    event = e_align_done;
  }
}

/***************************************************************************//*!
*
* @brief   RUN state
*
* @param   void
*
* @return  none
*
******************************************************************************/
static void stateRun(void)
{
  /* Application State Machine - state identification */
  state = run;
  event = e_run;
  
  /* Up/Down push buttons */
  if((switchUpHW == HW_SWITCH_ON) && (switchUpHWLast == HW_SWITCH_OFF))
  {
    if(speedRequiredRPM < SPEED_MAX_RPM - SPEED_INC_RPM)
    {
      speedRequiredRPM += SPEED_INC_RPM;
    }
    else
    {
      speedRequiredRPM = SPEED_MAX_RPM;
    }
  }
  if((switchDownHW == HW_SWITCH_ON) && (switchDownHWLast == HW_SWITCH_OFF))
  {
    if(speedRequiredRPM > -SPEED_MAX_RPM + SPEED_INC_RPM)
    {
      speedRequiredRPM -= SPEED_INC_RPM;
    }
    else
    {
      speedRequiredRPM = -SPEED_MAX_RPM;
    }
  }

  /* Calibrate resolver gains and DC-offsets */
  if(switchCalibrateRSLV == TURNED_ON)
  {
    fs_etpu_rslv_autocalibrate(pmsmrsvc_instance.RSLV_channel);
    switchCalibrateRSLV = TURNED_OFF;  
  }

  /* Set reguired speed */
  fs_etpu_app_pmsmrsvc_set_speed_required(&pmsmrsvc_instance, speedRequiredRPM);
}

/*********************************************************************
 *
 * Copyright:
 *	Freescale Semiconductor, INC. All Rights Reserved.
 *  You are hereby granted a copyright license to use, modify, and
 *  distribute the SOFTWARE so long as this entire notice is
 *  retained without alteration in any modified and/or redistributed
 *  versions, and that such modified versions are clearly identified
 *  as such. No licenses are granted by implication, estoppel or
 *  otherwise under any patents or trademarks of Freescale
 *  Semiconductor, Inc. This software is provided on an "AS IS"
 *  basis and without warranty.
 *
 *  To the maximum extent permitted by applicable law, Freescale
 *  Semiconductor DISCLAIMS ALL WARRANTIES WHETHER EXPRESS OR IMPLIED,
 *  INCLUDING IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A
 *  PARTICULAR PURPOSE AND ANY WARRANTY AGAINST INFRINGEMENT WITH
 *  REGARD TO THE SOFTWARE (INCLUDING ANY MODIFIED VERSIONS THEREOF)
 *  AND ANY ACCOMPANYING WRITTEN MATERIALS.
 *
 *  To the maximum extent permitted by applicable law, IN NO EVENT
 *  SHALL Freescale Semiconductor BE LIABLE FOR ANY DAMAGES WHATSOEVER
 *  (INCLUDING WITHOUT LIMITATION, DAMAGES FOR LOSS OF BUSINESS PROFITS,
 *  BUSINESS INTERRUPTION, LOSS OF BUSINESS INFORMATION, OR OTHER
 *  PECUNIARY LOSS) ARISING OF THE USE OR INABILITY TO USE THE SOFTWARE.
 *
 *  Freescale Semiconductor assumes no responsibility for the
 *  maintenance and support of this software
 ********************************************************************/
