/*******************************************************************************
Copyright (c) 2014 released Microchip Technology Inc. All rights reserved.

Microchip licenses to you the right to use, modify, copy and distribute
Software only when embedded on a Microchip microcontroller or digital signal
controller that is integrated into your product or third party product
(pursuant to the sublicense terms in the accompanying license agreement).

You should refer to the license agreement accompanying this Software for
additional information regarding your rights and obligations.

SOFTWARE AND DOCUMENTATION ARE PROVIDED AS IS WITHOUT WARRANTY OF ANY KIND,
EITHER EXPRESS OR IMPLIED, INCLUDING WITHOUT LIMITATION, ANY WARRANTY OF
MERCHANTABILITY, TITLE, NON-INFRINGEMENT AND FITNESS FOR A PARTICULAR PURPOSE.
IN NO EVENT SHALL MICROCHIP OR ITS LICENSORS BE LIABLE OR OBLIGATED UNDER
CONTRACT, NEGLIGENCE, STRICT LIABILITY, CONTRIBUTION, BREACH OF WARRANTY, OR
OTHER LEGAL EQUITABLE THEORY ANY DIRECT OR INDIRECT DAMAGES OR EXPENSES
INCLUDING BUT NOT LIMITED TO ANY INCIDENTAL, SPECIAL, INDIRECT, PUNITIVE OR
CONSEQUENTIAL DAMAGES, LOST PROFITS OR LOST DATA, COST OF PROCUREMENT OF
SUBSTITUTE GOODS, TECHNOLOGY, SERVICES, OR ANY CLAIMS BY THIRD PARTIES
(INCLUDING BUT NOT LIMITED TO ANY DEFENSE THEREOF), OR OTHER SIMILAR COSTS.
*******************************************************************************/

// *****************************************************************************
// *****************************************************************************
// Section: Included Files
// *****************************************************************************
// *****************************************************************************
#include <stdint.h>
#ifdef __XC16__  // See comments at the top of this header file
    #include <xc.h>
#endif // __XC16__

#include <libq.h>     
#include "motor_control.h"
#include "general.h"   
#include "user_parms.h" 
#include "motorA.h"
#include "motorB.h"
#include "periph.h"    
#include "readadc.h"   
#include "meascurr.h"  
#include "control.h"   
#include "estim.h"
#include "fdweak.h"
#include "MCP802x_DE2.h"
#include "RTDM.h"
#include "uart.h"

// *****************************************************************************
// *****************************************************************************
// Section: Variables
// *****************************************************************************
// *****************************************************************************

/******************** Variables used for Inverter A Control *******************/

/* Library Struct & Variable defines */

MC_ALPHABETA_T tClarkVoltageA,tClarkCurrentA;
MC_SINCOS_T tSinCosThetaA;
MC_DQ_T tParkVoltageA,tParkCurrentA;
MC_DUTYCYCLEOUT_T tPWMDutyValueA;
MC_ABC_T   tPhaseVoltagesA,tPhaseCurrentsA;
MC_PIPARMIN_T tPIParmInIQA;
MC_PIPARMOUT_T tPIParmOutIQA;
MC_PIPARMIN_T tPIParmInIDA;
MC_PIPARMOUT_T tPIParmOutIDA;
MC_PIPARMIN_T tPIParmInWA;
MC_PIPARMOUT_T tPIParmOutWA;

/* Other Variables & Structures */

CTRL_PARM_T tCtrlParmA;
MOTOR_PARM_T tMotorParmA;
volatile UGF_T tUGFlagsA;
ESTIM_PARM_T tEstimParmA;
MOTOR_ESTIM_PARM_T tMotorEstimParmA;
MEAS_CURR_PARM_T tMeasCurrParmA;         
READ_ADC_PARM_T tReadADCParmA;   
BUTTON_T tBtnStartStopA,tBtnHalfDoubleA;
ADC_OFFSET_T iADC1Offset;
volatile int16_t qRotorAngleMA = 0,qAngleOpenloopA = 0;
uint16_t iPWMPeriodA;

/******************** Variables used for Inverter B Control *******************/

/* Library Struct & Variable defines */

MC_ALPHABETA_T tClarkVoltageB,tClarkCurrentB;
MC_SINCOS_T tSinCosThetaB;
MC_DQ_T tParkVoltageB,tParkCurrentB;
MC_DUTYCYCLEOUT_T tPWMDutyValueB;
MC_ABC_T   tPhaseVoltagesB,tPhaseCurrentsB;

MC_PIPARMIN_T tPIParmInIQB;
MC_PIPARMOUT_T tPIParmOutIQB;
MC_PIPARMIN_T tPIParmInIDB;
MC_PIPARMOUT_T tPIParmOutIDB;
MC_PIPARMIN_T tPIParmInWB;
MC_PIPARMOUT_T tPIParmOutWB;

/* Other Variables & Structures */

CTRL_PARM_T tCtrlParmB;
MOTOR_PARM_T tMotorParmB;
volatile UGF_T tUGFlagsB;
ESTIM_PARM_T tEstimParmB;
MOTOR_ESTIM_PARM_T tMotorEstimParmB;
MEAS_CURR_PARM_T tMeasCurrParmB;
READ_ADC_PARM_T tReadADCParmB;
BUTTON_T tBtnStartStopB,tBtnHalfDoubleB;
ADC_OFFSET_T iADC2Offset;
volatile int16_t qRotorAngleMB = 0,qAngleOpenloopB = 0;
uint16_t iPWMPeriodB;

uint16_t  iADCisrCnt = 0;

/******************** Variables used for RTDM/DMCI Commnication ***************/
#ifdef RTDM
/* Buffer to store the data samples for the DMCI data viewer Graph1 */
volatile int16_t RecorderBuffer1[DATA_BUFFER_SIZE];
/* Buffer to store the data samples for the DMCI data viewer Graph2 */
volatile int16_t RecorderBuffer2[DATA_BUFFER_SIZE];
/* Buffer to store the data samples for the DMCI data viewer Graph3 */
volatile int16_t RecorderBuffer3[DATA_BUFFER_SIZE];
/* Buffer to store the data samples for the DMCI data viewer Graph4 */
volatile int16_t RecorderBuffer4[DATA_BUFFER_SIZE];

/* Tail pointer for the DMCI Graph1 */
volatile int16_t * PtrRecBuffer1 = &RecorderBuffer1[0];
/* Tail pointer for the DMCI Graph2 */
volatile int16_t * PtrRecBuffer2 = &RecorderBuffer2[0];
/* Tail pointer for the DMCI Graph3 */
volatile int16_t * PtrRecBuffer3 = &RecorderBuffer3[0];
/* Tail pointer for the DMCI Graph4 */
volatile int16_t * PtrRecBuffer4 = &RecorderBuffer4[0];

/* Buffer Recorder Upper Limit */
volatile int16_t * RecBuffUpperLimit = RecorderBuffer1 + DATA_BUFFER_SIZE - 1;

typedef struct DMCIFlags
{
    /* Flag needs to be set to start buffering data */
    unsigned Recorder : 1;
    unsigned StartStop : 1;
    unsigned unused : 14;
} DMCIFLAGS;
volatile DMCIFLAGS DMCIFlags;

volatile int16_t SnapCount = 0;
volatile int16_t SnapShotDelayCnt = 0;
volatile int16_t SnapShotDelay = SNAPDELAY;

#endif // End of #ifdef RTDM

// *****************************************************************************
// *****************************************************************************
// Section: Local Definitions
// *****************************************************************************
// *****************************************************************************

/* maximum motor speed converted into electrical speed */
#define MA_MAXIMUMSPEED_ELECTR MA_MAXIMUM_SPEED_RPM*MA_NOPOLESPAIRS
/* nominal motor speed converted into electrical speed */
#define MA_NOMINALSPEED_ELECTR MA_NOMINAL_SPEED_RPM*MA_NOPOLESPAIRS

/* end speed conveted to fit the startup ramp */
#define MA_END_SPEED (MA_END_SPEED_RPM * MA_NOPOLESPAIRS * LOOPTIME_SEC * 65536 / 60.0)*1024
/* end speed of open loop ramp up converted into electrical speed */
#define MA_ENDSPEED_ELECTR MA_END_SPEED_RPM*MA_NOPOLESPAIRS

/* maximum motor speed converted into electrical speed */
#define MB_MAXIMUMSPEED_ELECTR MB_MAXIMUM_SPEED_RPM*MB_NOPOLESPAIRS
/* nominal motor speed converted into electrical speed */
#define MB_NOMINALSPEED_ELECTR MB_NOMINAL_SPEED_RPM*MB_NOPOLESPAIRS

/* end speed conveted to fit the startup ramp */
#define MB_END_SPEED (MB_END_SPEED_RPM * MB_NOPOLESPAIRS * LOOPTIME_SEC * 65536 / 60.0)*1024
/* end speed of open loop ramp up converted into electrical speed */
#define MB_ENDSPEED_ELECTR MB_END_SPEED_RPM*MB_NOPOLESPAIRS
// Number of control loops that must execute before the button 
// routine is executed.
#define	DISPLOOPTIME	(DISPLOOPTIME_SEC/LOOPTIME_SEC)

/* maximum pot value in sfrac mode 0xFFC0 */
/* shifted left with 1 meaning 16368 */ 
#define  MAXPOTVAL_SHL1 16368

// *****************************************************************************
// *****************************************************************************
// Section: Local Functions
// *****************************************************************************
// *****************************************************************************

void __attribute__((__interrupt__)) _AD1Interrupt(void);
void __attribute__((__interrupt__)) _AD2Interrupt(void);
static void InitControlParameters(uint16_t);
static void DoControl(uint16_t );
static void CalculateParkAngle(uint16_t);
void ButtonScan(BUTTON_T *);
void ResetParmsA(void);
void ResetParmsB(void);
void InitPeripherals(void);
void DBGSnapUpdate(void);

// *****************************************************************************
/* Function:
   main()

  Summary:
    main() function

  Description:
    program entry point, calls the system init function group containing the
    buttons polling loop

  Precondition:
    None.

  Parameters:
    None

  Returns:
    None.

  Remarks:
    None.
 */
int main ( void )
{

    InitOscillator();

    SetupGPIOPorts();

    CORCONbits.SATA  = 0;

    /* Setting CE_A = 0 */
    CE_A_MCP802x = DISABLE;
    /* Setting CE_B = 0 */
    CE_B_MCP802x = DISABLE;
    DelayinMicrosecond(1);

#ifdef MOTOR_A
    /* Setting CE_A = 1 */
    CE_A_MCP802x = ENABLE;

    /* init PI control parameters */
    InitControlParameters(MOTOR_A);

    /* init estim parameters */
    InitEstimParm(MOTOR_A);

    /* init flux weakening params */
    InitFWParams(MOTOR_A);

    /* Configure MCP8024 driver - Inverter A */
    ConfigureMcp8024(DE2_UART_MCP8024_A);
#endif
#ifdef MOTOR_B

    /* Setting CE_B = 1 */
    CE_B_MCP802x = ENABLE;

    /* init PI control parameters */
    InitControlParameters(MOTOR_B);

    /* init estim parameters */
    InitEstimParm(MOTOR_B);

    /* init flux weakening params */
    InitFWParams(MOTOR_B);

    /* Configure MCP8024 driver - Inverter A */
    ConfigureMcp8024(DE2_UART_MCP8024_B);

#endif

#ifdef RTDM
    DMCIFlags.Recorder = 0;

    /* Configure the UART module used by RTDM, it also initializes the RTDM
       variables */
    RTDM_Start();

#endif

    InitPeripherals();

    while(1)
    {

#ifdef MOTOR_A
        ResetParmsA();
#endif
#ifdef MOTOR_B
        ResetParmsB();
#endif
        while(1)
        {
            #ifdef RTDM
                RTDM_ProcessMsgs();
            #endif

            #ifdef MOTOR_A
            /* Button scanning loop to start/Stop motor */
            tBtnStartStopA.member.value = BTN_START_STOP_A;
            ButtonScan(&tBtnStartStopA);
            if(tBtnStartStopA.member.status == BTN_PRESSED)
            {
                tBtnStartStopA.member.status = BTN_NOT_PRESSED;
                if(tUGFlagsA.bits.RunMotor == 1)		
                {
                    ResetParmsA();
                }
                else
                {
                    tUGFlagsA.bits.RunMotor = 1;
                }

            }
            if(tUGFlagsA.bits.RunMotor == 1)
            {
                /* Button scanning loop to enter into field weakening mode */
                tBtnHalfDoubleA.member.value = BTN_SPD_HALF_DBL_A;
                ButtonScan(&tBtnHalfDoubleA);
                if(tBtnHalfDoubleA.member.status == BTN_PRESSED)
                {
                    tBtnHalfDoubleA.member.status = BTN_NOT_PRESSED;
                    tUGFlagsA.bits.ChangeSpeed = !tUGFlagsA.bits.ChangeSpeed;

                }

            }
            #endif
            #ifdef MOTOR_B
            /* Button scanning loop to start/Stop motor */
            tBtnStartStopB.member.value = BTN_START_STOP_B;
            ButtonScan(&tBtnStartStopB);
            if(tBtnStartStopB.member.status == BTN_PRESSED)
            {
                tBtnStartStopB.member.status = BTN_NOT_PRESSED;
                if(tUGFlagsB.bits.RunMotor == 1)		
                {
                    ResetParmsB();
                }
                else
                {
                    tUGFlagsB.bits.RunMotor = 1;
                }

            }
            if(tUGFlagsB.bits.RunMotor == 1)
            {
                /* Button scanning loop to enter into field weakening mode */
                tBtnHalfDoubleB.member.value = BTN_SPD_HALF_DBL_B;
                ButtonScan(&tBtnHalfDoubleB);
                if(tBtnHalfDoubleB.member.status == BTN_PRESSED)
                {
                    tBtnHalfDoubleB.member.status = BTN_NOT_PRESSED;
                    tUGFlagsB.bits.ChangeSpeed = !tUGFlagsB.bits.ChangeSpeed;

                }

            }
            #endif
            if (iADCisrCnt ==  30000)
            {
                iADCisrCnt =  0;
                if(tUGFlagsB.bits.RunMotor == 1)
                {
                    /* To verify the DE2 status and take action */
                    StatusCheckMcp802x(DE2_UART_MCP8024_B);
                }
                if(tUGFlagsA.bits.RunMotor == 1)
                {
                    /* To verify the DE2 status and take action */
                    StatusCheckMcp802x(DE2_UART_MCP8024_A);
                }
            }
        }

    } // End of Main loop

    // should never get here
    while(1){}
}
// *****************************************************************************
/* Function:
    Button_Scan()

  Summary:
    Routine to scan the push buttons on the board

  Description:
    Function scans the pushbutton and indicates the status in the structure

  Precondition:
    None.

  Parameters:
    pointer to button structure

  Returns:
    None.

  Remarks:
    None.
 */
void ButtonScan(BUTTON_T *pButton)
{
    if(pButton->member.value == 0)
    {
        if(pButton->member.debounce_count < BTN_DEBOUNCE_COUNT)
        {
            pButton->member.debounce_count++;
            pButton->member.status = BTN_DEBOUNCE;
        }
    }
    else
    {
        if(pButton->member.debounce_count < BTN_DEBOUNCE_COUNT)
        {
            pButton->member.status = BTN_NOT_PRESSED;
        }
        else
        {
           pButton->member.status = BTN_PRESSED;
        }
        pButton->member.debounce_count = 0;
    }
}
#ifdef MOTOR_A
// *****************************************************************************
/* Function:
    ResetParmsA()

  Summary:
    This routine resets all the paramenters requuired for Motor through Inv-A

  Description:
    Reintilises the duty cyle,resets all the ocunters etc when restarting motor

  Precondition:
    None.

  Parameters:
    None

  Returns:
    None.

  Remarks:
    None.
 */
void ResetParmsA(void)
{

    /* Re initialise the duty cycle to minimum value */
    INVERTERA_PWM_PDC1 = LOOPTIME_TCY-MIN_DUTY;
    INVERTERA_PWM_PDC2 = LOOPTIME_TCY-MIN_DUTY;
    INVERTERA_PWM_PDC3 = LOOPTIME_TCY-MIN_DUTY;

    /* Make sure adc generate interrupt while initialising parameters */
    DisableADC1Interrupt();

    /* Stop the motor   */
    tUGFlagsA.bits.RunMotor = 0;

    tUGFlagsA.Word = 0;
    tBtnStartStopA.Word = 0;
    tBtnHalfDoubleA.Word = 0;

    /* Set the reference speed value to 0 */
    tCtrlParmA.qVelRef = 0;
    /* Restart in open loop */
    tUGFlagsA.bits.OpenLoop = 1;
    /* Change speed */
    tUGFlagsA.bits.ChangeSpeed = 0;
    /* Change mode */
    tUGFlagsA.bits.ChangeMode = 0;

    /* Zero out i sums */
    tPIParmInIQA.piState.integrator = 0;
    tPIParmInIDA.piState.integrator = 0;
    tPIParmInWA.piState.integrator = 0;

    /* Init PI control parameters */
    InitControlParameters(MOTOR_A);
    /* Init estim parameters */
    InitEstimParm(MOTOR_A);
    /* Init flux weakening params */
    InitFWParams(MOTOR_A);

    /* Enable ADC interrupt and begin main loop timing */
    ClearADC1IF();
    EnableADC1Interrupt();

}
#endif
#ifdef MOTOR_B
// *****************************************************************************
/* Function:
    ResetParmsB()

  Summary:
    This routine resets all the paramenters requuired for Motor through Inv-A

  Description:
    Reintilises the duty cyle,resets all the ocunters etc when restarting motor

  Precondition:
    None.

  Parameters:
    None

  Returns:
    None.

  Remarks:
    None.
 */
void ResetParmsB(void)
{
    /* Re initialise the duty cycle to minimum value */
    INVERTERB_PWM_PDC1 = MIN_DUTY;
    INVERTERB_PWM_PDC2 = MIN_DUTY;
    INVERTERB_PWM_PDC3 = MIN_DUTY;

    /* Make sure adc generate interrupt while initialising parameters */
    DisableADC2Interrupt();

    /* Stop the motor   */
    tUGFlagsB.Word = 0;
    tUGFlagsB.bits.RunMotor = 0;

    tBtnStartStopB.Word = 0;
    tBtnHalfDoubleB.Word = 0;

    /* Set the reference speed value to 0 */
    tCtrlParmB.qVelRef = 0;
    /* Restart in open loop */
    tUGFlagsB.bits.OpenLoop = 1;
    /* Change speed */
    tUGFlagsB.bits.ChangeSpeed = 0;
    /* Change mode */
    tUGFlagsB.bits.ChangeMode = 0;

    /* Zero out i sums */
    tPIParmInIQB.piState.integrator = 0;
    tPIParmInIDB.piState.integrator = 0;
    tPIParmInWB.piState.integrator = 0;

    /* Init PI control parameters */
    InitControlParameters(MOTOR_B);
    /* Init estim parameters */
    InitEstimParm(MOTOR_B);
    /* Init flux weakening params */
    InitFWParams(MOTOR_B);

    /* Enable ADC interrupt and begin main loop timing */
    ClearADC2IF();
    EnableADC2Interrupt();

}
#endif

// *****************************************************************************
/* Function:
    DoControl()

  Summary:
    Executes one PI itteration for each of the three loops Id,Iq,Speed

  Description:
    This routine executes one PI itteration for each of the three loops
    Id,Iq,Speed

  Precondition:
    None.

  Parameters:
    None

  Returns:
    None.

  Remarks:
    None.
 */
static void DoControl( unsigned int Motor_Number )
{
    /* Temporary vars for sqrt calculation of q reference */
    volatile int16_t temp_qref_pow_q15;

    #ifdef  MOTOR_A
    if(Motor_Number == MOTOR_A)
    {
        #ifdef TUNING_A
            /* Tuning speed ramp value  */
            tMotorParmA.tuning_add_rampup = 0;
            /* Tuning speed ramp increase delay */
            tMotorParmA.tuning_delay_rampup;   
        #endif

        if( tUGFlagsA.bits.OpenLoop )
        {
            /* OPENLOOP:  force rotating angle,Vd,Vq */
            if( tUGFlagsA.bits.ChangeMode )
            {
                /* Just changed to openloop */
                tUGFlagsA.bits.ChangeMode = 0;

                /* Synchronize angles */
                /* VqRef & VdRef not used */
                tCtrlParmA.qVqRef = 0;
                tCtrlParmA.qVdRef = 0;

                /* Reinit vars for initial speed ramp */
                tMotorParmA.startup_Lock = 0;
                tMotorParmA.startup_Ramp = 0;
                #ifdef TUNING_A
                tMotorParmA.tuning_add_rampup = 0;
                #endif
            }
            /* Speed reference */
            tCtrlParmA.qVelRef = MA_Q_CURRENT_REF_OPENLOOP;
            /* q current reference is equal to the vel reference
             while d current reference is equal to 0
            for maximum startup torque, set the q current to maximum acceptable
            value represents the maximum peak value */
            tCtrlParmA.qVqRef    = tCtrlParmA.qVelRef;

            /* PI control for Q */
            tPIParmInIQA.inMeasure  = tParkCurrentA.q;
            tPIParmInIQA.inReference  = tCtrlParmA.qVqRef;
            MC_ControllerPIUpdate_Assembly(tPIParmInIQA.inReference,
                                           tPIParmInIQA.inMeasure,
                                           &tPIParmInIQA.piState,
                                           &tPIParmOutIQA.out);;
            tParkVoltageA.q = tPIParmOutIQA.out;

            /* PI control for D */
            tPIParmInIDA.inMeasure = tParkCurrentA.d;
            tPIParmInIDA.inReference = tCtrlParmA.qVdRef;
            MC_ControllerPIUpdate_Assembly(tPIParmInIDA.inReference,
                                           tPIParmInIDA.inMeasure,
                                           &tPIParmInIDA.piState,
                                           &tPIParmOutIDA.out);
            tParkVoltageA.d = tPIParmOutIDA.out;

        }
        /* Closed Loop Vector Control */
        else
        {
            /* if change speed indication, double the speed */
            if(tUGFlagsA.bits.ChangeSpeed)
            {
                /* if Bidirectional functioning, disable speed doubling */
                #ifndef BIDIRECTIONAL_SPEED_A
                    /* Read not signed ADC */
                    ReadADC0(ADCBUF_SPEED_REF_A,&tReadADCParmA );

                    tReadADCParmA.qAnRef = (tReadADCParmA.qADValue);
                    if(tReadADCParmA.qAnRef > MA_MAXIMUMSPEED_ELECTR)
                    {
                        tReadADCParmA.qAnRef = MA_MAXIMUMSPEED_ELECTR;
                    }
                #else

                    /* Unsigned values */
                    ReadSignedADC0(ADCBUF_SPEED_REF_A,&tReadADCParmA );

                    /* ADC values are shifted with 2 */
                    /* Speed pot ref max value +-8190 */
                    tReadADCParmA.qAnRef = tReadADCParmA.qADValue>>2;
                #endif
            }
            else
            {
                /* if Bidirectional, read pot value signed */
                #ifdef BIDIRECTIONAL_SPEED_A
                    /* Signed values, different than in case of FW above */
                    ReadSignedADC0(ADCBUF_SPEED_REF_A,&tReadADCParmA );
                #else
                    /* Unsigned values */
                    ReadADC0(ADCBUF_SPEED_REF_A,&tReadADCParmA );
                #endif
                /* ADC values are shifted with 2 */
                /* Speed pot ref max value +-8190 */
                tReadADCParmA.qAnRef=tReadADCParmA.qADValue>>2;		
            }

            /* Ramp generator to limit the change of the speed reference
              the rate of change is defined by CtrlParm.qRefRamp */
            tCtrlParmA.qDiff=tCtrlParmA.qVelRef - tReadADCParmA.qAnRef;
            /* Speed Ref Ramp */
            if (tCtrlParmA.qDiff < 0)
            {
                /* Set this cycle reference as the sum of
                previously calculated one plus the reframp value */
                tCtrlParmA.qVelRef=tCtrlParmA.qVelRef+tCtrlParmA.qRefRamp;
            }
            else
            {
                /* Same as above for speed decrease */
                tCtrlParmA.qVelRef=tCtrlParmA.qVelRef-tCtrlParmA.qRefRamp;
            }
            /* If difference less than half of ref ramp, set reference
            directly from the pot */
            if (_Q15abs(tCtrlParmA.qDiff) < (tCtrlParmA.qRefRamp<<1))
            {
                tCtrlParmA.qVelRef = tReadADCParmA.qAnRef;
            }

            /* Tuning is generating a software ramp
            with sufficiently slow ramp defined by
            TUNING_DELAY_RAMPUP constant */
            #ifdef TUNING_A
            /* if delay is not completed */
            if(tMotorParmA.tuning_delay_rampup > TUNING_DELAY_RAMPUP_A)
            {
                tMotorParmA.tuning_delay_rampup = 0;
            }
            /* While speed less than maximum and delay is complete */
            if((tMotorParmA.tuning_add_rampup < (MA_MAXIMUMSPEED_ELECTR - MA_ENDSPEED_ELECTR)) &&
                                                (tMotorParmA.tuning_delay_rampup == 0) )
            {
                /* Increment ramp add */
                tMotorParmA.tuning_add_rampup++;
            }
            tMotorParmA.tuning_delay_rampup++;
            /* The reference is continued from the open loop speed up ramp */
            tCtrlParmA.qVelRef =   MA_ENDSPEED_ELECTR +
                                   tMotorParmA.tuning_add_rampup;
            #endif

            if( tUGFlagsA.bits.ChangeMode )
            {
                /* Just changed from openloop */
                tUGFlagsA.bits.ChangeMode = 0;
                tPIParmInWA.piState.integrator = (uint32_t)tCtrlParmA.qVqRef << 13;
            }

            /* If TORQUE MODE skip the speed controller */
            #ifndef	TORQUE_MODE_A
                /* Execute the velocity control loop */
                tPIParmInWA.inMeasure  = tEstimParmA.qVelEstim;
                tPIParmInWA.inReference  = tCtrlParmA.qVelRef;
                MC_ControllerPIUpdate_Assembly(tPIParmInWA.inReference,
                                               tPIParmInWA.inMeasure,
                                               &tPIParmInWA.piState,
                                               &tPIParmOutWA.out);;
                tCtrlParmA.qVqRef = tPIParmOutWA.out;
            #else
                tCtrlParmA.qVqRef = tCtrlParmA.qVelRef;
            #endif

            /* Flux weakenign control - the actual speed is replaced
            with the reference speed for stability
            reference for d curent component
            adapt the estimator parameters in concordance with the speed */
            tCtrlParmA.qVdRef = FieldWeakening(MOTOR_A,_Q15abs(tCtrlParmA.qVelRef));

            /* PI control for D */
            tPIParmInIDA.inMeasure = tParkCurrentA.d;
            tPIParmInIDA.inReference = tCtrlParmA.qVdRef;
            MC_ControllerPIUpdate_Assembly(tPIParmInIDA.inReference,
                                           tPIParmInIDA.inMeasure,
                                           &tPIParmInIDA.piState,
                                           &tPIParmOutIDA.out);
            tParkVoltageA.d    = tPIParmOutIDA.out;

            /* Dynamic d-q adjustment
             with d component priority
             vq=sqrt (vs^2 - vd^2)
             limit vq maximum to the one resulting from the calculation above */
            temp_qref_pow_q15 = (int16_t)(__builtin_mulss(tPIParmOutIDA.out ,
                                          tPIParmOutIDA.out)>>15);
            temp_qref_pow_q15 = Q15(0.98) - temp_qref_pow_q15;
            tPIParmInIQA.piState.outMax = Q15SQRT (temp_qref_pow_q15);

            /* PI control for Q */
            tPIParmInIQA.inMeasure = tParkCurrentA.q;
            tPIParmInIQA.inReference = tCtrlParmA.qVqRef;
            MC_ControllerPIUpdate_Assembly(tPIParmInIQA.inReference,
                                           tPIParmInIQA.inMeasure,
                                           &tPIParmInIQA.piState,
                                           &tPIParmOutIQA.out);;
            tParkVoltageA.q = tPIParmOutIQA.out;
        }
    }
    #endif
    #ifdef  MOTOR_B
    if(Motor_Number == MOTOR_B)
    {
        #ifdef TUNING_B
            /* Tuning speed ramp value  */
            tMotorParmB.tuning_add_rampup = 0;
            /* Tuning speed ramp increase delay */
            tMotorParmB.tuning_delay_rampup;
        #endif

        if( tUGFlagsB.bits.OpenLoop )
        {
            /* OPENLOOP:  force rotating angle,Vd,Vq */
            if( tUGFlagsB.bits.ChangeMode )
            {
                /* Just changed to openloop */
                tUGFlagsB.bits.ChangeMode = 0;

                /* Synchronize angles */
                /* VqRef & VdRef not used */
                tCtrlParmB.qVqRef = 0;
                tCtrlParmB.qVdRef = 0;

                /* Reinit vars for initial speed ramp */
                tMotorParmB.startup_Lock = 0;
                tMotorParmB.startup_Ramp = 0;
                #ifdef TUNING_B
                tMotorParmB.tuning_add_rampup = 0;
                #endif
            }
            /* Speed reference */
            tCtrlParmB.qVelRef = MB_Q_CURRENT_REF_OPENLOOP;
            /* q current reference is equal to the vel reference
             while d current reference is equal to 0
            for maximum startup torque, set the q current to maximum acceptable
            value represents the maximum peak value */
            tCtrlParmB.qVqRef    = tCtrlParmB.qVelRef;

            /* PI control for Q */
            tPIParmInIQB.inMeasure  = tParkCurrentB.q;
            tPIParmInIQB.inReference  = tCtrlParmB.qVqRef;
            MC_ControllerPIUpdate_Assembly(tPIParmInIQB.inReference,
                                           tPIParmInIQB.inMeasure,
                                           &tPIParmInIQB.piState,
                                           &tPIParmOutIQB.out);;
            tParkVoltageB.q = tPIParmOutIQB.out;

            /* PI control for D */
            tPIParmInIDB.inMeasure = tParkCurrentB.d;
            tPIParmInIDB.inReference = tCtrlParmB.qVdRef;
            MC_ControllerPIUpdate_Assembly(tPIParmInIDB.inReference,
                                           tPIParmInIDB.inMeasure,
                                           &tPIParmInIDB.piState,
                                           &tPIParmOutIDB.out);
            tParkVoltageB.d = tPIParmOutIDB.out;

        }
        /* Closed Loop Vector Control */
        else
        {
            /* if change speed indication, double the speed */
            if(tUGFlagsB.bits.ChangeSpeed)
            {
                /* if Bidirectional functioning, disable speed doubling */
                #ifndef BIDIRECTIONAL_SPEED_B
                    /* Read not signed ADC */
                    ReadADC0(ADCBUF_SPEED_REF_B,&tReadADCParmB );

                    tReadADCParmB.qAnRef = (tReadADCParmB.qADValue);
                    if(tReadADCParmB.qAnRef > MB_MAXIMUMSPEED_ELECTR)
                    {
                        tReadADCParmB.qAnRef = MB_MAXIMUMSPEED_ELECTR;
                    }
                #else

                    /* Unsigned values */
                    ReadSignedADC0(ADCBUF_SPEED_REF_B,&tReadADCParmB );

                    /* ADC values are shifted with 2 */
                    /* Speed pot ref max value +-8190 */
                    tReadADCParmB.qAnRef = tReadADCParmB.qADValue>>2;
                #endif
            }
            else
            {
                /* if Bidirectional, read pot value signed */
                #ifdef BIDIRECTIONAL_SPEED_B
                    /* Signed values, different than in case of FW above */
                    ReadSignedADC0(ADCBUF_SPEED_REF_B,&tReadADCParmB );
                #else
                    /* Unsigned values */
                    ReadADC0(ADCBUF_SPEED_REF_B,&tReadADCParmB );
                #endif
                /* ADC values are shifted with 2 */
                /* Speed pot ref max value +-8190 */
                tReadADCParmB.qAnRef=tReadADCParmB.qADValue>>2;
            }

            /* Ramp generator to limit the change of the speed reference
              the rate of change is defined by CtrlParm.qRefRamp */
            tCtrlParmB.qDiff=tCtrlParmB.qVelRef - tReadADCParmB.qAnRef;
            /* Speed Ref Ramp */
            if (tCtrlParmB.qDiff < 0)
            {
                /* Set this cycle reference as the sum of
                previously calculated one plus the reframp value */
                tCtrlParmB.qVelRef=tCtrlParmB.qVelRef+tCtrlParmB.qRefRamp;
            }
            else
            {
                /* Same as above for speed decrease */
                tCtrlParmB.qVelRef=tCtrlParmB.qVelRef-tCtrlParmB.qRefRamp;
            }
            /* If difference less than half of ref ramp, set reference
            directly from the pot */
            if (_Q15abs(tCtrlParmB.qDiff) < (tCtrlParmB.qRefRamp<<1))
            {
                tCtrlParmB.qVelRef = tReadADCParmB.qAnRef;
            }

            /* Tuning is generating a software ramp
            with sufficiently slow ramp defined by
            TUNING_DELAY_RAMPUP constant */
            #ifdef TUNING_B
            /* if delay is not completed */
            if(tMotorParmB.tuning_delay_rampup > TUNING_DELAY_RAMPUP_B)
            {
                tMotorParmB.tuning_delay_rampup = 0;
            }
            /* While speed less than maximum and delay is complete */
            if((tMotorParmB.tuning_add_rampup < (MB_MAXIMUMSPEED_ELECTR - MB_ENDSPEED_ELECTR)) &&
                                                (tMotorParmB.tuning_delay_rampup == 0) )
            {
                /* Increment ramp add */
                tMotorParmB.tuning_add_rampup++;
            }
            tMotorParmB.tuning_delay_rampup++;
            /* The reference is continued from the open loop speed up ramp */
            tCtrlParmB.qVelRef =   MB_ENDSPEED_ELECTR +
                                   tMotorParmB.tuning_add_rampup;
            #endif

            if( tUGFlagsB.bits.ChangeMode )
            {
                /* Just changed from openloop */
                tUGFlagsB.bits.ChangeMode = 0;
                tPIParmInWB.piState.integrator = (uint32_t)tCtrlParmB.qVqRef << 13;
            }

            /* If TORQUE MODE skip the speed controller */
            #ifndef	TORQUE_MODE_B
                /* Execute the velocity control loop */
                tPIParmInWB.inMeasure  = tEstimParmB.qVelEstim;
                tPIParmInWB.inReference  = tCtrlParmB.qVelRef;
                MC_ControllerPIUpdate_Assembly(tPIParmInWB.inReference,
                                               tPIParmInWB.inMeasure,
                                               &tPIParmInWB.piState,
                                               &tPIParmOutWB.out);;
                tCtrlParmB.qVqRef = tPIParmOutWB.out;
            #else
                tCtrlParmB.qVqRef = tCtrlParmB.qVelRef;
            #endif

            /* Flux weakenign control - the actual speed is replaced
            with the reference speed for stability
            reference for d curent component
            adapt the estimator parameters in concordance with the speed */
            tCtrlParmB.qVdRef = FieldWeakening(MOTOR_B,_Q15abs(tCtrlParmB.qVelRef));

            /* PI control for D */
            tPIParmInIDB.inMeasure = tParkCurrentB.d;
            tPIParmInIDB.inReference = tCtrlParmB.qVdRef;
            MC_ControllerPIUpdate_Assembly(tPIParmInIDB.inReference,
                                           tPIParmInIDB.inMeasure,
                                           &tPIParmInIDB.piState,
                                           &tPIParmOutIDB.out);
            tParkVoltageB.d    = tPIParmOutIDB.out;

            /* Dynamic d-q adjustment
             with d component priority
             vq=sqrt (vs^2 - vd^2)
             limit vq maximum to the one resulting from the calculation above */
            temp_qref_pow_q15 = (int16_t)(__builtin_mulss(tPIParmOutIDB.out ,
                                          tPIParmOutIDB.out)>>15);
            temp_qref_pow_q15 = Q15(0.98) - temp_qref_pow_q15;
            tPIParmInIQB.piState.outMax = Q15SQRT (temp_qref_pow_q15);

            /* PI control for Q */
            tPIParmInIQB.inMeasure = tParkCurrentB.q;
            tPIParmInIQB.inReference = tCtrlParmB.qVqRef;
            MC_ControllerPIUpdate_Assembly(tPIParmInIQB.inReference,
                                           tPIParmInIQB.inMeasure,
                                           &tPIParmInIQB.piState,
                                           &tPIParmOutIQB.out);;
            tParkVoltageB.q = tPIParmOutIQB.out;
        }
    }
    #endif
}
// *****************************************************************************
/* Function:
    ADC1Interrupt()

  Summary:
    ADC1Interrupt() ISR routine

  Description:
    Does speed calculation and executes the vector update loop
    The ADC sample and conversion is triggered by the PWM period.
    The speed calculation assumes a fixed time interval between calculations.

  Precondition:
    None.

  Parameters:
    None

  Returns:
    None.

  Remarks:
    None.
 */
void __attribute__((interrupt, no_auto_psv)) _AD1Interrupt(void)
{
    /* Clear Interrupt Flag */
    ClearADC1IF();
    
    iADCisrCnt++;

    #ifdef  MOTOR_A
    if( tUGFlagsA.bits.RunMotor )
    {
	/* Calculate qIa,qIb */
        MeasCompCurr(ADCBUF_INV_A_IPHASE1, ADCBUF_INV_A_IPHASE2,&tMeasCurrParmA);
        tPhaseCurrentsA.a = tMeasCurrParmA.qIa;
        tPhaseCurrentsA.b = tMeasCurrParmA.qIb;
        /* Calculate qId,qIq from qSin,qCos,qIa,qIb */
        MC_TransformClarke_Assembly(&tPhaseCurrentsA,&tClarkCurrentA);
        MC_TransformPark_Assembly(&tClarkCurrentA,&tSinCosThetaA,
                                   &tParkCurrentA);

        /* Speed and field angle estimation */
        Estim(MOTOR_A);
        /* Calculate control values */
        DoControl(MOTOR_A);
	CalculateParkAngle(MOTOR_A);
        /* If open loop */
        if(tUGFlagsA.bits.OpenLoop == 1)
        {
            /* The angle is given by parkparm */
            qRotorAngleMA = qAngleOpenloopA;

        }
        else
        {
            /* If closed loop, angle generated by estim */
            qRotorAngleMA = tEstimParmA.qRho;
        }

        MC_CalculateSineCosine_Assembly_Ram(qRotorAngleMA,&tSinCosThetaA);
        MC_TransformParkInverse_Assembly(&tParkVoltageA,&tSinCosThetaA,
                                                        &tClarkVoltageA);

        MC_TransformClarkeInverseSwappedInput_Assembly(&tClarkVoltageA,
                                                       &tPhaseVoltagesA);
        MC_CalculateSpaceVectorPhaseShifted_Assembly(&tPhaseVoltagesA,
                                                     iPWMPeriodA,
                                                     &tPWMDutyValueA);

        if(tPWMDutyValueA.dutycycle1 < MIN_DUTY)
        {
            tPWMDutyValueA.dutycycle1 = MIN_DUTY;
        }
        if(tPWMDutyValueA.dutycycle2 < MIN_DUTY)
        {
            tPWMDutyValueA.dutycycle2 = MIN_DUTY;
        }
        if(tPWMDutyValueA.dutycycle3 < MIN_DUTY)
        {
            tPWMDutyValueA.dutycycle3 = MIN_DUTY;
        }

        INVERTERA_PWM_PDC1 = LOOPTIME_TCY - tPWMDutyValueA.dutycycle1;
        INVERTERA_PWM_PDC2 = LOOPTIME_TCY - tPWMDutyValueA.dutycycle2;
	INVERTERA_PWM_PDC3 = LOOPTIME_TCY - tPWMDutyValueA.dutycycle3;
    }
    #endif
    /* Record DMCI Dynamic Data Views  */
    #ifdef RTDM
        DBGSnapUpdate();
    #endif
}
// *****************************************************************************
/* Function:
    ADC2Interrupt()

  Summary:
    ADC2Interrupt() ISR routine

  Description:
    Does speed calculation and executes the vector update loop
    The ADC sample and conversion is triggered by the PWM period.
    The speed calculation assumes a fixed time interval between calculations.

  Precondition:
    None.

  Parameters:
    None

  Returns:
    None.

  Remarks:
    None.
 */
void __attribute__((interrupt, no_auto_psv)) _AD2Interrupt(void)
{
    /* Clear Interrupt Flag */
    ClearADC2IF();

    #ifdef  MOTOR_B
    if( tUGFlagsB.bits.RunMotor )
    {
	/* Calculate qIa,qIb */
        MeasCompCurr(ADCBUF_INV_B_IPHASE1, ADCBUF_INV_B_IPHASE2,&tMeasCurrParmB);
        tPhaseCurrentsB.a = tMeasCurrParmB.qIa;
        tPhaseCurrentsB.b = tMeasCurrParmB.qIb;

        /* Calculate qId,qIq from qSin,qCos,qIa,qIb */
        MC_TransformClarke_Assembly(&tPhaseCurrentsB,&tClarkCurrentB);
        MC_TransformPark_Assembly(&tClarkCurrentB,&tSinCosThetaB,
                                   &tParkCurrentB);

        /* Speed and field angle estimation */
        Estim(MOTOR_B);
        /* Calculate control values */
        DoControl(MOTOR_B);
        /* Calculate qAngle from QEI Module */
	CalculateParkAngle(MOTOR_B);
        /* If open loop */
        if(tUGFlagsB.bits.OpenLoop == 1)
        {
            /* The angle is given by parkparm */
            qRotorAngleMB = qAngleOpenloopB;
        }
        else
        {
            /* If closed loop, angle generated by estim */
            qRotorAngleMB = tEstimParmB.qRho;
        }

        MC_CalculateSineCosine_Assembly_Ram(qRotorAngleMB,&tSinCosThetaB);
        MC_TransformParkInverse_Assembly(&tParkVoltageB,&tSinCosThetaB,
                                                        &tClarkVoltageB);

        MC_TransformClarkeInverseSwappedInput_Assembly(&tClarkVoltageB,
                                                       &tPhaseVoltagesB);
        MC_CalculateSpaceVectorPhaseShifted_Assembly(&tPhaseVoltagesB,
                                                     iPWMPeriodB,
                                                     &tPWMDutyValueB);

        if(tPWMDutyValueB.dutycycle1 < MIN_DUTY)
        {
            tPWMDutyValueB.dutycycle1 = MIN_DUTY;
        }
        if(tPWMDutyValueB.dutycycle2 < MIN_DUTY)
        {
            tPWMDutyValueB.dutycycle2 = MIN_DUTY;
        }
        if(tPWMDutyValueB.dutycycle3 < MIN_DUTY)
        {
            tPWMDutyValueB.dutycycle3 = MIN_DUTY;
        }

        INVERTERB_PWM_PDC1 = tPWMDutyValueB.dutycycle1;
        INVERTERB_PWM_PDC2 = tPWMDutyValueB.dutycycle2;
	INVERTERB_PWM_PDC3 = tPWMDutyValueB.dutycycle3;
    }
    #endif
}
// *****************************************************************************
/* Function:
    CalculateParkAngle ()

  Summary:
    Function calulates the angle for open loop control

  Description:
    Generate the start sinwaves feeding the motor's terminals
    Open loop control, forcing the motor to allign and to start speeding up .

  Precondition:
    None.

  Parameters:
    None

  Returns:
    None.

  Remarks:
    None.
 */
static void CalculateParkAngle(uint16_t Motor_Number)
{
    #ifdef  MOTOR_A
    if(Motor_Number == MOTOR_A)
    {
        /* If open loop */
        if(tUGFlagsA.bits.OpenLoop)
        {
            /* Begin wiht the lock sequence, for field alligniament */
            if (tMotorParmA.startup_Lock < MA_LOCK_TIME)
            {
                tMotorParmA.startup_Lock+= 1;
            }
            /* Then ramp up till the end speed */
            else if (tMotorParmA.startup_Ramp < MA_END_SPEED)
            {
                tMotorParmA.startup_Ramp+= MA_OPENLOOP_RAMPSPEED_INCREASERATE;
            }
            /* Switch to closed loop */
            else
            {
                #ifndef OPEN_LOOP_FUNCTIONING_A

                tUGFlagsA.bits.ChangeMode = 1;
                tUGFlagsA.bits.OpenLoop = 0;

                #endif
            }
            /* The angle set depends on startup ramp */
            qAngleOpenloopA+= (int16_t)(tMotorParmA.startup_Ramp >> 10);

        }
        /* Switched to closed loop */
        else
        {
            /* In closed loop slowly decrease the offset add to the estimated
               angle */
            if(tEstimParmA.qRhoOffset>0)
            {
                tEstimParmA.qRhoOffset--;
            }
        }
        return;
    }
    #endif
    #ifdef  MOTOR_B

    if(Motor_Number == MOTOR_B)
    {
        /* if open loop */
        if(tUGFlagsB.bits.OpenLoop)
        {
            /* Begin wiht the lock sequence, for field alligniament */
            if (tMotorParmB.startup_Lock < MB_LOCK_TIME)
            {
                tMotorParmB.startup_Lock+= 1;
            }
            /* then ramp up till the end speed */
            else if (tMotorParmB.startup_Ramp < MB_END_SPEED)
            {
                tMotorParmB.startup_Ramp+= MB_OPENLOOP_RAMPSPEED_INCREASERATE;
            }
            /* Switch to closed loop */
            else
            {
                #ifndef OPEN_LOOP_FUNCTIONING_B

                tUGFlagsB.bits.ChangeMode = 1;
                tUGFlagsB.bits.OpenLoop = 0;

                #endif
            }
            /* The angle set depends on startup ramp */
            qAngleOpenloopB+= (int16_t)(tMotorParmB.startup_Ramp >> 10);

        }
        /* Switched to closed loop */
        else
        {
            /* In closed loop slowly decrease the offset add to the estimated
               angle */
            if(tEstimParmB.qRhoOffset>0)
            {
                tEstimParmB.qRhoOffset--;
            }
        }
        return;
    }
    #endif
}
// *****************************************************************************
/* Function:
    InitControlParameters()

  Summary:
    Function initialises control parameters

  Description:
    Init control parameters: PI coefficients, scalling consts etc.

  Precondition:
    None.

  Parameters:
    None

  Returns:
    None.

  Remarks:
    None.
 */
static void InitControlParameters(unsigned int Motor_Number)
{
    #ifdef  MOTOR_A
    if(Motor_Number == MOTOR_A)
    {
        /* ADC - Measure Current & Pot */
        /* Scaling constants: Determined by calibration or hardware design.*/
        tReadADCParmA.qK      = MA_KPOT;
        tMeasCurrParmA.qKa    = MA_KCURRA;
        tMeasCurrParmA.qKb    = MA_KCURRB;

        tCtrlParmA.qRefRamp = MA_SPEEDREFRAMP;

        /* SVGen  */
        /* Set PWM period to Loop Time */
        iPWMPeriodA = LOOPTIME_TCY;

        /* PI D Term */
        tPIParmInIDA.piState.kp = MA_D_CURRCNTR_PTERM;
        tPIParmInIDA.piState.ki = MA_D_CURRCNTR_ITERM;
        tPIParmInIDA.piState.kc = MA_D_CURRCNTR_CTERM;
        tPIParmInIDA.piState.outMax = MA_D_CURRCNTR_OUTMAX;
        tPIParmInIDA.piState.outMin = -tPIParmInIDA.piState.outMax;
        tPIParmInIDA.piState.integrator = 0;
        tPIParmOutIDA.out = 0;

        /* PI Q Term */
        tPIParmInIQA.piState.kp = MA_Q_CURRCNTR_PTERM;
        tPIParmInIQA.piState.ki = MA_Q_CURRCNTR_ITERM;
        tPIParmInIQA.piState.kc = MA_Q_CURRCNTR_CTERM;
        tPIParmInIQA.piState.outMax = MA_Q_CURRCNTR_OUTMAX;
        tPIParmInIQA.piState.outMin = -tPIParmInIQA.piState.outMax;
        tPIParmInIQA.piState.integrator = 0;
        tPIParmOutIQA.out = 0;

        /* PI W Term */
        tPIParmInWA.piState.kp = MA_SPEEDCNTR_PTERM;
        tPIParmInWA.piState.ki = MA_SPEEDCNTR_ITERM;
        tPIParmInWA.piState.kc = MA_SPEEDCNTR_CTERM;
        tPIParmInWA.piState.outMax = MA_SPEEDCNTR_OUTMAX;
        tPIParmInWA.piState.outMin = -tPIParmInWA.piState.outMax;
        tPIParmInWA.piState.integrator = 0;
        tPIParmOutWA.out = 0;
        return;
    }
    #endif
    #ifdef  MOTOR_B
    if(Motor_Number == MOTOR_B)
    {
        /* ADC - Measure Current & Pot */
        /* Scaling constants: Determined by calibration or hardware design.*/
        tReadADCParmB.qK      = MB_KPOT;
        tMeasCurrParmB.qKa    = MB_KCURRA;
        tMeasCurrParmB.qKb    = MB_KCURRB;

        tCtrlParmB.qRefRamp = MB_SPEEDREFRAMP;

        /* SVGen  */
        /* Set PWM period to Loop Time */
        iPWMPeriodB = LOOPTIME_TCY;

        /* PI D Term */
        tPIParmInIDB.piState.kp = MB_D_CURRCNTR_PTERM;
        tPIParmInIDB.piState.ki = MB_D_CURRCNTR_ITERM;
        tPIParmInIDB.piState.kc = MB_D_CURRCNTR_CTERM;
        tPIParmInIDB.piState.outMax = MB_D_CURRCNTR_OUTMAX;
        tPIParmInIDB.piState.outMin = -tPIParmInIDB.piState.outMax;
        tPIParmInIDB.piState.integrator = 0;
        tPIParmOutIDB.out = 0;

        /* PI Q Term */
        tPIParmInIQB.piState.kp = MB_Q_CURRCNTR_PTERM;
        tPIParmInIQB.piState.ki = MB_Q_CURRCNTR_ITERM;
        tPIParmInIQB.piState.kc = MB_Q_CURRCNTR_CTERM;
        tPIParmInIQB.piState.outMax = MB_Q_CURRCNTR_OUTMAX;
        tPIParmInIQB.piState.outMin = -tPIParmInIQB.piState.outMax;
        tPIParmInIQB.piState.integrator = 0;
        tPIParmOutIQB.out = 0;

        /* PI W Term */
        tPIParmInWB.piState.kp = MB_SPEEDCNTR_PTERM;
        tPIParmInWB.piState.ki = MB_SPEEDCNTR_ITERM;
        tPIParmInWB.piState.kc = MB_SPEEDCNTR_CTERM;
        tPIParmInWB.piState.outMax = MB_SPEEDCNTR_OUTMAX;
        tPIParmInWB.piState.outMin = -tPIParmInWB.piState.outMax;
        tPIParmInWB.piState.integrator = 0;
        tPIParmOutWB.out = 0;
        return;
    }
    #endif
}
// *****************************************************************************
/* Function:
    InitPeripherals()

  Summary:
    Routine initalises controller peripherals

  Description:
    Routine to initailise Peripherals used for Inverter Control

  Precondition:
    None.

  Parameters:
    None

  Returns:
    None.

  Remarks:
    None.
 */
void InitPeripherals(void)
{
    /* Initialise comparator and amplifiers */
    InitAmplifiersComparator();
    /* Initialise PWM generators */
    InitPWMGenerators();
    /* Initialise the ADC used for sensing Inverter A paramneters */
    InitADCModule1(&iADC1Offset);
    /* Initialising Current offsets in structure variable */
    InitMeasCompCurr(iADC1Offset.ch2,
                     iADC1Offset.ch1, &tMeasCurrParmA);

    /* Initialise the ADC used for sensing Inverter B paramneters */
    InitADCModule2(&iADC2Offset);
    /* Initialising Current offsets in structure variable */
    InitMeasCompCurr(iADC2Offset.ch2,
                     iADC2Offset.ch3, &tMeasCurrParmB);

    DelayinMicrosecond(1);
}
// *****************************************************************************
/* Function:
    DBG_SnapUpdate ()

  Summary:
    Snap the values sent to host for debug purposes

  Description:
    Snap the values sent to host for debug purposes update the internal debbuger
    variables (snap buffers) at certain timing

  Precondition:
    None.

  Parameters:
    None

  Returns:
    None.

  Remarks:
    None.
 */
void DBGSnapUpdate(void)
{
    /* DMCI Dynamic Data Views  */
    #ifdef RTDM
    if (DMCIFlags.Recorder)
    {
        SnapShotDelayCnt++;
        if (SnapShotDelayCnt == SnapShotDelay)
        {
            SnapShotDelayCnt = 0;
            *PtrRecBuffer1++ = SNAP1;
            *PtrRecBuffer2++ = SNAP2;
            *PtrRecBuffer3++ = SNAP3;
            *PtrRecBuffer4++ = SNAP4;

            if (PtrRecBuffer1 > RecBuffUpperLimit)
            {
                PtrRecBuffer1 = RecorderBuffer1;
                PtrRecBuffer2 = RecorderBuffer2;
                PtrRecBuffer3 = RecorderBuffer3;
                PtrRecBuffer4 = RecorderBuffer4;
                DMCIFlags.Recorder = 0;
            }
        }
    }
    #endif
}


/* EOF */