

/**
\defgroup tiescappl tiescappl.c: TI ESC Application
\brief V5.0 : create file


\version 5.0
*/
/**
 * tiescappl.c
 *
*/
/*
 * Copyright (c) 2015, Texas Instruments Incorporated
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * *  Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * *  Neither the name of Texas Instruments Incorporated nor the names of
 *    its contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 **/
//---------------------------------------------------------------------------------------
/**
\ingroup tiescappl
\file tiescappl.c
\brief Implementation.
*/
//---------------------------------------------------------------------------------------

/*-----------------------------------------------------------------------------------------
------
------    Includes
------
-----------------------------------------------------------------------------------------*/
#include "ecat_def.h"

#if TIESC_APPLICATION

#if COE_SUPPORTED
    #include "objdef.h"
#endif
#include "ecatappl.h"

#include "tiescutils.h"

#define _TIESC_HW_
#include "ecatslv.h"
#include "tiescappl.h"
#undef _TIESC_HW_

#ifdef AM43XX_FAMILY_BUILD
    #include "endat_drv.h"

    #include <IQmathLib.h>
#endif

extern boardId_t boardType;
extern PRUICSS_Handle pruIcss1Handle;
/*--------------------------------------------------------------------------------------
------
------    local types and defines
------
--------------------------------------------------------------------------------------*/

/*-----------------------------------------------------------------------------------------
------
------    local variables and constants
------
-----------------------------------------------------------------------------------------*/

/*-----------------------------------------------------------------------------------------
------
------    application specific functions
------
-----------------------------------------------------------------------------------------*/
extern Uint32 appState;
/*-----------------------------------------------------------------------------------------
------
------    generic functions
------
-----------------------------------------------------------------------------------------*/
/////////////////////////////////////////////////////////////////////////////////////////
/**

\brief This function resets the outputs
*////////////////////////////////////////////////////////////////////////////////////////

void PDO_ResetOutputs(void)
{
    sDOOutputs.LEDs = 0x00;
    sDO1Outputs.Cmd = 0x00;
    sDO1Outputs.Count = 0x00;
    sDO1Outputs.MotorData = 0x1000;
}

/////////////////////////////////////////////////////////////////////////////////////////
/**
 \brief    The function is called when an error state was acknowledged by the master

*////////////////////////////////////////////////////////////////////////////////////////

void    APPL_AckErrorInd(Uint16 stateTrans)
{

}
/*ECATCHANGE_START(V4.42.1) ECAT2*/
/////////////////////////////////////////////////////////////////////////////////////////
/**
 \return    AL Status Code (see ecatslv.h ALSTATUSCODE_....)

 \brief    The function is called in the state transition from INIT to PREOP when
             all general settings were checked to start the mailbox handler. This function
             informs the application about the state transition, the application can refuse
             the state transition when returning an AL Status error code.
            The return code NOERROR_INWORK can be used, if the application cannot confirm
            the state transition immediately, in that case this function will be called cyclically
            until a value unequal NOERROR_INWORK is returned

*////////////////////////////////////////////////////////////////////////////////////////

Uint16 APPL_StartMailboxHandler(void)
{
    return ALSTATUSCODE_NOERROR;
}

/////////////////////////////////////////////////////////////////////////////////////////
/**
 \return     0, NOERROR_INWORK

 \brief    The function is called in the state transition from PREEOP to INIT
             to stop the mailbox handler. This functions informs the application
             about the state transition, the application cannot refuse
             the state transition.

*////////////////////////////////////////////////////////////////////////////////////////

Uint16 APPL_StopMailboxHandler(void)
{
    return ALSTATUSCODE_NOERROR;
}

/////////////////////////////////////////////////////////////////////////////////////////
/**
 \param    pIntMask    pointer to the AL Event Mask which will be written to the AL event Mask
                        register (0x204) when this function is succeeded. The event mask can be adapted
                        in this function
 \return    AL Status Code (see ecatslv.h ALSTATUSCODE_....)

 \brief    The function is called in the state transition from PREOP to SAFEOP when
             all general settings were checked to start the input handler. This function
             informs the application about the state transition, the application can refuse
             the state transition when returning an AL Status error code.
             When returning ALSTATUSCODE_NOERROR, the inputs has to be updated once before return.
            The return code NOERROR_INWORK can be used, if the application cannot confirm
            the state transition immediately, in that case the application need to be complete
            the transition by calling ECAT_StateChange
*////////////////////////////////////////////////////////////////////////////////////////

Uint16 APPL_StartInputHandler(Uint16 *pIntMask)
{
	/*	check if latches are assgned to PDI */
//	Uint8 cyclicUnitControl = bsp_read_byte(pruIcss1Handle, 0x0980 );
//	if( cyclicUnitControl & (1 << 4) )
//	{
//		bsp_write_byte( pruIcss1Handle, 0x03, 0x09A8);
//	}
//	if( cyclicUnitControl & (1 << 5) )
//	{
//		bsp_write_byte( pruIcss1Handle, 0x03, 0x09A9);
//	}

	/*	write lacth0 nd latch1 configuration */
	bsp_write_word( pruIcss1Handle, 0x0303, 0x09A8 );
	*((unsigned int*)0x4a32e010) = 0x0001FCC0 ;
//	HW_EscWriteWord( 0x09A8, 0x0303 );

	bsp_pdi_write_systime_delay(pruIcss1Handle,1);

	/*	Add Latch Event*/
	*pIntMask |= 0x0002 ;

    return ALSTATUSCODE_NOERROR;
}

/////////////////////////////////////////////////////////////////////////////////////////
/**
 \return     0, NOERROR_INWORK

 \brief    The function is called in the state transition from SAFEOP to PREEOP
             to stop the input handler. This functions informs the application
             about the state transition, the application cannot refuse
             the state transition.

*////////////////////////////////////////////////////////////////////////////////////////

Uint16 APPL_StopInputHandler(void)
{
    #if ESC_EEPROM_EMULATION

    if(BOARD_ICEV2 == boardType)
    {
        /*
            Since both SPI Flash and HVS (input) are on the same SPI instance, application will have to make sure that
            access to both does not happen concurrently. At this context, it is ensured that input(HVS) read wont happen.
        */
        if(bsp_get_eeprom_update_status())
        {
            bsp_set_eeprom_update_status(0);
            bsp_eeprom_emulation_flush();
        }
    }

    #endif
    return ALSTATUSCODE_NOERROR;
}

/////////////////////////////////////////////////////////////////////////////////////////
/**
 \return    AL Status Code (see ecatslv.h ALSTATUSCODE_....)

 \brief    The function is called in the state transition from SAFEOP to OP when
             all general settings were checked to start the output handler. This function
             informs the application about the state transition, the application can refuse
             the state transition when returning an AL Status error code.
            The return code NOERROR_INWORK can be used, if the application cannot confirm
            the state transition immediately, in that case this function will be called cyclically
            until a value unequal NOERROR_INWORK is returned
*////////////////////////////////////////////////////////////////////////////////////////

Uint16 APPL_StartOutputHandler(void)
{
    /*always return NOERROR_INWORK the state transition will be finished by calling AL_ControlRes*/
    return ALSTATUSCODE_NOERROR;
}

/////////////////////////////////////////////////////////////////////////////////////////
/**
 \return     0, NOERROR_INWORK

 \brief    The function is called in the state transition from OP to SAFEOP
             to stop the output handler. This functions informs the application
             about the state transition, the application cannot refuse
             the state transition.

*////////////////////////////////////////////////////////////////////////////////////////

Uint16 APPL_StopOutputHandler(void)
{
    PDO_ResetOutputs();
    return ALSTATUSCODE_NOERROR;
}

/////////////////////////////////////////////////////////////////////////////////////////
/**
\return     0(ALSTATUSCODE_NOERROR), NOERROR_INWORK
\param      pInputSize  pointer to save the input process data length
\param      pOutputSize  pointer to save the output process data length

\brief    This function calculates the process data sizes from the actual SM-PDO-Assign
            and PDO mapping
*////////////////////////////////////////////////////////////////////////////////////////
Uint16 APPL_GenerateMapping(Uint16 *pInputSize, Uint16 *pOutputSize)
{
    Uint16 result = 0;
    Uint16 PDOAssignEntryCnt = 0;
    OBJCONST TOBJECT OBJMEM *pPDO = NULL;
    Uint16 PDOSubindex0 = 0;
    UINT32 *pPDOEntry = NULL;
    Uint16 PDOEntryCnt = 0;
    Uint16 InputSize = 0;
    Uint16 OutputSize = 0;

    /*Scan object 0x1C12 RXPDO assign*/
    for(PDOAssignEntryCnt = 0; PDOAssignEntryCnt < sRxPDOassign.u16SubIndex0;
            PDOAssignEntryCnt++)
    {
        pPDO = OBJ_GetObjectHandle(sRxPDOassign.aEntries[PDOAssignEntryCnt]);

        if(pPDO != NULL)
        {
            PDOSubindex0 = *((Uint16 *)pPDO->pVarPtr);

            for(PDOEntryCnt = 0; PDOEntryCnt < PDOSubindex0; PDOEntryCnt++)
            {
                pPDOEntry = (UINT32 *)((Uint8 *)pPDO->pVarPtr + (OBJ_GetEntryOffset((
                                           PDOEntryCnt + 1), pPDO) >> 3));     //goto PDO entry
                // we increment the expected output size depending on the mapped Entry
                OutputSize += (Uint16)((*pPDOEntry) & 0xFF);
            }
        }
    }

    OutputSize = (OutputSize + 7) >> 3;

    /*Scan Object 0x1C13 TXPDO assign*/
    for(PDOAssignEntryCnt = 0; PDOAssignEntryCnt < sTxPDOassign.u16SubIndex0;
            PDOAssignEntryCnt++)
    {
        pPDO = OBJ_GetObjectHandle(sTxPDOassign.aEntries[PDOAssignEntryCnt]);

        if(pPDO != NULL)
        {
            PDOSubindex0 = *((Uint16 *)pPDO->pVarPtr);

            for(PDOEntryCnt = 0; PDOEntryCnt < PDOSubindex0; PDOEntryCnt++)
            {
                pPDOEntry = (UINT32 *)((Uint8 *)pPDO->pVarPtr + (OBJ_GetEntryOffset((
                                           PDOEntryCnt + 1), pPDO) >> 3));     //goto PDO entry
                // we increment the expected output size depending on the mapped Entry
                InputSize += (Uint16)((*pPDOEntry) & 0xFF);
            }
        }
    }

    InputSize = (InputSize + 7) >> 3;

    *pInputSize = InputSize;
    *pOutputSize = OutputSize;
    return result;
}


/////////////////////////////////////////////////////////////////////////////////////////
/**
\param      pData  pointer to input process data
\brief      This function will copies the inputs from the local memory to the ESC memory
            to the hardware
*////////////////////////////////////////////////////////////////////////////////////////
void APPL_InputMapping(Uint16 *pData)
{
    Uint16 j = 0;
    Uint8 *pTmpData = (Uint8 *)pData;


    for(j = 0; j < sTxPDOassign.u16SubIndex0; j++)
    {
        switch(sTxPDOassign.aEntries[j])
        {
            /* TxPDO 1 */
            case 0x1A00:
                *pTmpData++ = sDIInputs.switchs;
                break;

            case 0x1A03: // attention: maybe a non-aligned byte access...
                *pTmpData++ = sAI1Inputs.info1 & 0xFF;
                *pTmpData++ = (sAI1Inputs.info1 & 0xFF00) >> 8;
                *pTmpData++ = (sAI1Inputs.info1 & 0xFF0000) >> 16;
                *pTmpData++ = (sAI1Inputs.info1 & 0xFF000000) >> 24;
                *pTmpData++ = sAI1Inputs.info2 & 0xFF;
                *pTmpData++ = (sAI1Inputs.info2 & 0xFF00) >> 8;
                break;
        }
    }
}

/////////////////////////////////////////////////////////////////////////////////////////
/**
\param      pData  pointer to output process data

\brief    This function will copies the outputs from the ESC memory to the local memory
            to the hardware
*////////////////////////////////////////////////////////////////////////////////////////
void APPL_OutputMapping(Uint16 *pData)
{
    Uint16 j = 0;
    Uint8 *pTmpData = (Uint8 *)pData;   // allow byte processing

    for(j = 0; j < sRxPDOassign.u16SubIndex0; j++)
    {
        switch(sRxPDOassign.aEntries[j])
        {
            /* RxPDO 2 */
            case 0x1601:
                sDOOutputs.LEDs = *pTmpData++;
                break;

            case 0x1602:
                sDO1Outputs.Count = *pTmpData++;
                sDO1Outputs.Cmd   = *pTmpData++;
                sDO1Outputs.MotorData = *pTmpData++;
                sDO1Outputs.MotorData |= (*pTmpData++ << 8);
                break;
        }
    }
}
#ifdef AM43XX_FAMILY_BUILD
    /////////////////////////////////////////////////////////////////////////////////////////
    /**
    \brief    This function will called from the synchronisation ISR
    or from the mainloop if no synchronisation is supported
    *////////////////////////////////////////////////////////////////////////////////////////
    extern volatile int ECAT_ChannelSel;
    extern volatile _iq ECAT_SpeedRef;
    extern volatile _iq ECAT_lsw;

#endif
extern PRUICSS_Handle pruIcss1Handle;

int foc_get_angle(void);
void update_timings( UInt64 previousLatch )
{
	sSynchronisationFsm.latchTimeDifference = sSynchronisationFsm.latch0PositiveEdge - previousLatch ;
	sSynchronisationFsm.referenceTime += sSynchronisationFsm.nominalCycleTime ;
	sSynchronisationFsm.referenceTimeError = -(sSynchronisationFsm.referenceTime - sSynchronisationFsm.latch0PositiveEdge) ;
}

void sync_fsm_state_handler()
{
	UInt i = Hwi_disable();
	static UInt64 previousLatch ;

	switch(sSynchronisationFsm.syncFsmState)
	{
	case 0:		/*	Stopped */
		break;

	case 1:		/*	Wait for first event */
		previousLatch = sSynchronisationFsm.latch0PositiveEdge ;
		sSynchronisationFsm.referenceTime = sSynchronisationFsm.latch0PositiveEdge ;
		if( sSynchronisationFsm.syncFsmCommand == 1 )
			sSynchronisationFsm.syncFsmState = 2 ;
		else
			sSynchronisationFsm.syncFsmState = 3 ;
		break;

	case 2:
		update_timings(previousLatch);
		previousLatch = sSynchronisationFsm.latch0PositiveEdge ;
		break;

	case 3:
		sSynchronisationFsm.latchTimeDifference = sSynchronisationFsm.latch0PositiveEdge - previousLatch ;
		sSynchronisationFsm.referenceTimeError = -(sSynchronisationFsm.referenceTime - sSynchronisationFsm.latch0PositiveEdge) ;
//#if defined(SYSTEM_TIME_PDI_CONTROLLED) || defined(FORCE_SYSTEM_TIME_PDI_CONTROLLED_API)
//		bsp_pdi_write_system_time( pruIcss1Handle, sSynchronisationFsm.referenceTimeUints[0]);
//#endif
		previousLatch = sSynchronisationFsm.latch0PositiveEdge ;
		break;

	default:
		sSynchronisationFsm.syncFsmState = 0 ;
		break;
	}

	Hwi_restore(i);
}

void sync_fsm_command_handler()
{
	UInt i = Hwi_disable();

	switch(sSynchronisationFsm.syncFsmState)
	{
	case 0:
		switch(sSynchronisationFsm.syncFsmCommand)
		{
		case 1:
		case 2:
			sSynchronisationFsm.syncFsmState = 1;
			break;

		default:
			break;
		}
		break;

	case 1:
	case 2:
	case 3:
		switch(sSynchronisationFsm.syncFsmCommand)
		{
		case 0:
			sSynchronisationFsm.syncFsmState = 0 ;
			sSynchronisationFsm.referenceTimeError = 0 ;
			sSynchronisationFsm.latchTimeDifference = 0 ;
			break;
		default:
			break;
		}
		break;

	default:
		break;
	}

	Hwi_restore(i);
}

//	PSEUDO CODE
//void latch_0_positive_edge_event_handler()
//{
//	static int is_first_event = 1 ;
//	static UInt64 reference_time = 0 ;
//	static UInt64 nominal_ttl_period = 1000000;
//
//	if( is_first_event )
//	{
//		reference_time = bsp_get_latch0_posedge_time() ;
//		is_first_event = 0 ;
//	}
//	else
//	{
//		reference_time += nominal_ttl_period ;
//		bsp_pdi_write_system_time( reference_time );
//	}
//}

void check_latch_registers()
{
	Uint16 latchreg = bsp_read_word( pruIcss1Handle, 0x09AE);

	if( latchreg & 0x0001 )
	{
#if defined(SYSTEM_TIME_PDI_CONTROLLED) || defined(FORCE_SYSTEM_TIME_PDI_CONTROLLED_API)
		if( sSynchronisationFsm.syncFsmState == 3 )
		{
			if( sSynchronisationFsm.options & 0x00000001 )
			{
				bsp_read( pruIcss1Handle, (Uint8*)(&sSynchronisationFsm.referenceTime), 0x0990, 8 );
				sSynchronisationFsm.referenceTime -= sSynchronisationFsm.nominalCycleTime - sSynchronisationFsm.sync0ToLatchdelay;
			}
			else
			{
				sSynchronisationFsm.referenceTime += sSynchronisationFsm.nominalCycleTime ;
			}

			bsp_pdi_write_system_time( pruIcss1Handle, sSynchronisationFsm.referenceTimeUints[0] );
		}
#endif
		/*	Latch 0 positive edge */
		bsp_get_latch0_posedge_time(pruIcss1Handle, (Uint32*)(&sSynchronisationFsm.latch0PositiveEdge), (Uint32*)(&sSynchronisationFsm.latch0PositiveEdge)+1);
		sync_fsm_state_handler();
	}

	if( latchreg & 0x0002 )
	{
		/*	Latch 0 negative edge */
		bsp_get_latch0_negedge_time(pruIcss1Handle, (Uint32*)(&sSynchronisationFsm.latch0NegativeEdge), (Uint32*)(&sSynchronisationFsm.latch0NegativeEdge)+1);
	}

	if( latchreg & 0x0100 )
	{
		/*	Latch 1 positive edge */
		bsp_get_latch1_posedge_time(pruIcss1Handle, (Uint32*)(&sSynchronisationFsm.latch1PositiveEdge), (Uint32*)(&sSynchronisationFsm.latch1PositiveEdge)+1);
	}

	if( latchreg & 0x0200 )
	{
		/*	Latch 2 negative edge */
		bsp_get_latch1_negedge_time(pruIcss1Handle, (Uint32*)(&sSynchronisationFsm.latch1NegativeEdge), (Uint32*)(&sSynchronisationFsm.latch1NegativeEdge)+1);
	}


}

void APPL_Latch()
{
	check_latch_registers();
}

void APPL_Application(void)
{
    Uint8 LED; // initial test data
    static Uint8 prevState = 55;

    sync_fsm_command_handler();

    LED = sDOOutputs.LEDs;

    if(LED != prevState)
    {
        Board_setDigOutput(LED);
    }

    prevState = LED;
    appState = sDO1Outputs.Cmd;// set the application state

    Board_getDigInput(&sDIInputs.switchs);

    if(appState == 0)
    {
        appState =
            sDIInputs.switchs;    //special mode to control app state by input switchs!
    }

    #ifdef AM43XX_FAMILY_BUILD
    sAI1Inputs.info1 = foc_get_angle();

    // Motor / FOC exchange

    if((sDO1Outputs.Cmd >= 0) && (sDO1Outputs.Cmd <= 2))
    {
        ECAT_lsw = sDO1Outputs.Cmd;
    }

    else
    {
        ECAT_lsw = 0;
    }


    if((sDO1Outputs.MotorData >= -1500) && (sDO1Outputs.MotorData <= 1500))
    {
        ECAT_SpeedRef = _IQ(((float) sDO1Outputs.MotorData) / 1000);
    }

    else
    {
        ECAT_SpeedRef = 0;
    }

    ECAT_ChannelSel = sDO1Outputs.Count;
    #elif defined(AM335X_FAMILY_BUILD)
    sAI1Inputs.info1 = 0x12345600 | LED;
    sAI1Inputs.info2 = bsp_read_word(pruIcss1Handle, 0x10);
    #endif

}

#if EXPLICIT_DEVICE_ID
/////////////////////////////////////////////////////////////////////////////////////////
/**
 \return    The Explicit Device ID of the EtherCAT slave

 \brief     Calculate the Explicit Device ID
*////////////////////////////////////////////////////////////////////////////////////////
Uint16 APPL_GetDeviceID()
{
    return 0xBABA;
}
#endif

/////////////////////////////////////////////////////////////////////////////////////////
/**

 \brief    This is the main function

*////////////////////////////////////////////////////////////////////////////////////////
int main()
{
    common_main();
    return 0;
}
/*ECATCHANGE_END(V4.42.1) ECAT2*/
#endif //#if TIESC_HW

/** @} */
