void InitApp(void)
{
    /* Initialize peripherals */

    // Configure UART modules
    UARTConfigure(UART_CMD_MODULE_ID, UART_ENABLE_PINS_TX_RX_ONLY);
    UARTSetFifoMode(UART_CMD_MODULE_ID, UART_INTERRUPT_ON_TX_NOT_FULL | UART_INTERRUPT_ON_RX_NOT_EMPTY);
    UARTSetLineControl(UART_CMD_MODULE_ID, UART_DATA_SIZE_8_BITS | UART_PARITY_NONE | UART_STOP_BITS_1);
    UARTSetDataRate(UART_CMD_MODULE_ID, GetPeripheralClock(), DESIRED_CMD_BAUDRATE);
    UARTEnable(UART_CMD_MODULE_ID, UART_ENABLE_FLAGS(UART_PERIPHERAL | UART_RX | UART_TX));

    UARTConfigure(UART_WIFI_MODULE_ID, UART_ENABLE_PINS_TX_RX_ONLY);
    UARTSetFifoMode(UART_WIFI_MODULE_ID, UART_INTERRUPT_ON_TX_NOT_FULL | UART_INTERRUPT_ON_RX_NOT_EMPTY);
    UARTSetLineControl(UART_WIFI_MODULE_ID, UART_DATA_SIZE_8_BITS | UART_PARITY_NONE | UART_STOP_BITS_1);
    UARTSetDataRate(UART_WIFI_MODULE_ID, GetPeripheralClock(), DESIRED_WIFI_BAUDRATE);
    UARTEnable(UART_WIFI_MODULE_ID, UART_ENABLE_FLAGS(UART_PERIPHERAL | UART_RX | UART_TX));

    // Configure UART Interrupts
    INTEnable(INT_SOURCE_UART_RX(UART_CMD_MODULE_ID), INT_ENABLED);
    INTSetVectorPriority(INT_VECTOR_UART(UART_CMD_MODULE_ID), INT_PRIORITY_LEVEL_2);
    INTSetVectorSubPriority(INT_VECTOR_UART(UART_CMD_MODULE_ID), INT_SUB_PRIORITY_LEVEL_0);

    INTEnable(INT_SOURCE_UART_RX(UART_WIFI_MODULE_ID), INT_ENABLED);
    INTSetVectorPriority(INT_VECTOR_UART(UART_WIFI_MODULE_ID), INT_PRIORITY_LEVEL_1);
    INTSetVectorSubPriority(INT_VECTOR_UART(UART_WIFI_MODULE_ID), INT_SUB_PRIORITY_LEVEL_0);

    // Make the requests to Wifi service - they will be picked up when the service needs them
    ConnectToAccessPoint(&routerConnection, &DefaultWifiService);
    SendHttpRequest(&DnsDynamicHttpRequest, &DefaultWifiService);
}

//inicializa todas as configuraçoes de hardware
static void InitializeBoard(void)
{	
    // LEDs
    LED0_TRIS = 0;
    LED1_TRIS = 0;
    LED0_IO   = 1;
    LED1_IO   = 1;

    //botoes
    BUTTON0_TRIS = 1;
    BUTTON1_TRIS = 1;

    // Enable 4x/5x/96MHz PLL on PIC18F87J10, PIC18F97J60, PIC18F87J50, etc.
    OSCTUNE = 0x40;

    // Set up analog features of PORTA
    ADCON0 = 0b00001001;	//ADON, Channel 2 (AN2)
    ADCON1 = 0b00001100;	//VSS0 VDD0, AN0,AN1,AN2 is analog
    ADCON2 = 0xBE;		//Right justify, 20TAD ACQ time, Fosc/64 (~21.0kHz)

    // Disable internal PORTB pull-ups
    INTCON2bits.RBPU = 1;

    // Configure USART
    TXSTA = 0x20;
    RCSTA = 0x90;

    // See if we can use the high baud rate setting
    #if ((GetPeripheralClock()+2*BAUD_RATE)/BAUD_RATE/4 - 1) <= 255
    SPBRG = (GetPeripheralClock()+2*BAUD_RATE)/BAUD_RATE/4 - 1;
    TXSTAbits.BRGH = 1;
    #else	// Use the low baud rate setting
    SPBRG = (GetPeripheralClock()+8*BAUD_RATE)/BAUD_RATE/16 - 1;
    #endif

        PIE1bits.RCIE = 1;
    // Enable Interrupts
    RCONbits.IPEN = 1;		// Enable interrupt priorities
    INTCONbits.GIEH = 1;
    INTCONbits.GIEL = 1;

    // Do a calibration A/D conversion
    ADCON0bits.ADCAL = 1;
    ADCON0bits.GO = 1;
    while(ADCON0bits.GO);
    ADCON0bits.ADCAL = 0;

    #if defined(SPIRAM_CS_TRIS)
            SPIRAMInit();
    #endif
    #if defined(EEPROM_CS_TRIS)
            XEEInit();
    #endif
    #if defined(SPIFLASH_CS_TRIS)
            SPIFlashInit();
    #endif
}

void mySetLineCodingHandler(void)
{
    //If the request is not in a valid range
    if(cdc_notice.GetLineCoding.dwDTERate.Val > 115200)
    {
        //NOTE: There are two ways that an unsupported baud rate could be
        //handled.  The first is just to ignore the request and don't change
        //the values.  That is what is currently implemented in this function.
        //The second possible method is to stall the STATUS stage of the request.
        //STALLing the STATUS stage will cause an exception to be thrown in the 
        //requesting application.  Some programs, like HyperTerminal, handle the
        //exception properly and give a pop-up box indicating that the request
        //settings are not valid.  Any application that does not handle the
        //exception correctly will likely crash when this requiest fails.  For
        //the sake of example the code required to STALL the status stage of the
        //request is provided below.  It has been left out so that this demo
        //does not cause applications without the required exception handling
        //to crash.
        //---------------------------------------
        //USBStallEndpoint(0,1);
    }
    else
    {
        DWORD_VAL dwBaud;

        //Update the baudrate info in the CDC driver
        CDCSetBaudRate(cdc_notice.GetLineCoding.dwDTERate.Val);
        
        //Update the baudrate of the UART
        #if defined(__18CXX)
            dwBaud.Val = (DWORD)(GetSystemClock()/4)/line_coding.dwDTERate.Val-1;
            SPBRG = dwBaud.v[0];
            SPBRGH = dwBaud.v[1];
        #elif defined(__C30__)
            #if defined(__dsPIC33EP512MU810__) || defined (__PIC24EP512GU810__)
            dwBaud.Val = ((GetPeripheralClock()/(unsigned long)(16 * line_coding.dwDTERate.Val)))- 1;
            #else
            dwBaud.Val = (((GetPeripheralClock()/2)+(BRG_DIV2/2*line_coding.dwDTERate.Val))/BRG_DIV2/line_coding.dwDTERate.Val-1);
            #endif
            U2BRG = dwBaud.Val;
        #elif defined(__C32__)
            U2BRG = ((GetPeripheralClock()+(BRG_DIV2/2*line_coding.dwDTERate.Val))/BRG_DIV2/line_coding.dwDTERate.Val-1);
            //U2MODE = 0;
            U2MODEbits.BRGH = BRGH2;
            //U2STA = 0;
        #endif
    }
}

/*********************************************************************
 * Function:        void UART2TCPBridgeInit(void)
 *
 * PreCondition:    None
 *
 * Input:           None
 *
 * Output:          None
 *
 * Side Effects:    None
 *
 * Overview:        Sets up the UART peripheral for this application
 *
 * Note:            Uses interrupts
 ********************************************************************/
void UART2TCPBridgeInit2(void)
{
	// Initilize UART

	putrsUART((ROM char*)"\r\n IN UART2TCPBridgeInit()");

    TXSTA2 = 0x20;
    RCSTA2 = 0x90;




	#define CLOSEST_SPBRG_VALUE ((GetPeripheralClock()+2ul*BAUD_RATE)/BAUD_RATE/4-1)
	#define BAUD_ACTUAL (GetPeripheralClock()/(CLOSEST_SPBRG_VALUE+1))
	#if (BAUD_ACTUAL > BAUD_RATE)
		#define BAUD_ERROR (BAUD_ACTUAL-BAUD_RATE)
	#else
		#define BAUD_ERROR (BAUD_RATE-BAUD_ACTUAL)
	#endif


	#define BAUD_ERROR_PRECENT	((BAUD_ERROR*100+BAUD_RATE/2)/BAUD_RATE)
	#if BAUD_ERROR_PRECENT > 2
		// Use high speed (Fosc/4) 16-bit baud rate generator
		//BAUDCONbits.BRG16 = 1;
		BRG162 = 1;
		//TXSTA2bits.BRGH = 1;
		BRGH2 = 1;
		SPBRGH2 = ((GetPeripheralClock()+BAUD_RATE/2)/BAUD_RATE-1)>>8 & 0xFF;
		SPBRG2 = ((GetPeripheralClock()+BAUD_RATE/2)/BAUD_RATE-1) & 0xFF;
	#else
		// See if we can use the high baud (Fosc/16) 8-bit rate setting
		#if ((GetPeripheralClock()+2*BAUD_RATE)/BAUD_RATE/4 - 1) <= 255
			SPBRG2 = (GetPeripheralClock()+2*BAUD_RATE)/BAUD_RATE/4 - 1;
			//TXSTA2bits.BRGH = 1;
			BRGH2 = 1;
		#else	// Use the low baud rate 8-bit setting
			SPBRG2 = (GetPeripheralClock()+8*BAUD_RATE)/BAUD_RATE/16 - 1;
		#endif
	#endif
	
	// Use high priority interrupt
	//IPR1bits.TXIP = 1;
	TX2IP = 1;

	RC2IE = 1;

}

void __attribute__ ((interrupt, no_auto_psv)) _INT2Interrupt(void)
{
static unsigned char odd_even;
	

	if(linear_cntr == 0)
	{
		rotate_detector = 1;
		debounce_cntr = 5;
		if(rotate_debug_max < rotate_det_cntr)
			rotate_debug_max = rotate_det_cntr;
		if(rotate_debug_min > rotate_det_cntr)
			rotate_debug_min = rotate_det_cntr;
		rotate_debug_last = rotate_det_cntr;
	}
//	if(odd_even)
	{
		odd_even = 0;
		if(linear_cntr != 0 || vol_para.rate > 700.0)
		{
			IFS1bits.INT2IF = 0; // Clear INT2 interrupt
			return;
		}
	
		
		linear_cntr = LINEAR_CORR;
// 		if(T2CONbits.TCKPS != 0)
		T2CONbits.TCKPS = 0;
// 		if ( PR2 > (unsigned int)(((GetPeripheralClock()/((float)MOTOR_STEPPERTURN*M_PULSEPERSTEP))*(3600.0*TENTHMLPERTURN))/700.0))
		PR2	= (unsigned int)(((GetPeripheralClock()/((float)MOTOR_STEPPERTURN*M_PULSEPERSTEP))*(3600.0*TENTHMLPERTURN))/700.0);
	}
//	else
//		odd_even = 1;
	IFS1bits.INT2IF = 0; // Clear INT2 interrupt
}

/**
 * \brief initialize an I2C interface using given config
 *
 * \param[in] hal - opaque ptr to HAL data
 * \param[in] cfg - interface configuration
 *
 * \return ATCA_STATUS
 */
ATCA_STATUS hal_i2c_init(void *hal, ATCAIfaceCfg *cfg)
{
    int bus = cfg->atcai2c.bus; // 0-based logical bus number
    int i;
    ATCAHAL_t *phal = (ATCAHAL_t*)hal;

    if (i2c_bus_ref_ct == 0)    // power up state, no i2c buses will have been used

    for (i = 0; i < MAX_I2C_BUSES; i++)
        i2c_hal_data[i] = NULL;

    i2c_bus_ref_ct++;   // total across buses

    if (bus >= 0 && bus < MAX_I2C_BUSES) 
    {
        //// if this is the first time this bus and interface has been created, do the physical work of enabling it
        if (i2c_hal_data[bus] == NULL) 
        {
            i2c_hal_data[bus] = malloc(sizeof(ATCAI2CMaster_t));
            i2c_hal_data[bus]->ref_ct = 1;  // buses are shared, this is the first instance

            switch (bus) 
            {
//            case 0:
//                i2c_hal_data[bus]->id = I2C0;
//                break;
            case 1:
                i2c_hal_data[bus]->id = I2C1;
                break;
//            case 2:
//                i2c_hal_data[bus]->id = I2C2;
//                break;
            case 3:
                i2c_hal_data[bus]->id = I2C3;
                break;
            }
            
            // Set the I2C baudrate
            I2CSetFrequency(i2c_hal_data[bus]->id, GetPeripheralClock(), cfg->atcai2c.baud);

            // Enable the I2C bus
            I2CEnable(i2c_hal_data[bus]->id, TRUE);
            
            // store this for use during the release phase
            i2c_hal_data[bus]->bus_index = bus;
        }
        else 
        {
            // otherwise, another interface already initialized the bus, so this interface will share it and any different
            // cfg parameters will be ignored...first one to initialize this sets the configuration
            i2c_hal_data[bus]->ref_ct++;
        }

        phal->hal_data = i2c_hal_data[bus];

        return ATCA_SUCCESS;
    }

    return ATCA_COMM_FAIL;
}

static WORD GetPrescale(DWORD Speed)
{
    double Div;
    WORD IntDiv;
    WORD T = 1;
    BYTE i = 0;
    BYTE PPRE = 0;
    BYTE SPRE = 0;
    BYTE Shift=0;    
    WORD SPIXCON1=0;
    
    Div=GetPeripheralClock()/((double)Speed);
    IntDiv=(WORD)Div+1;
    while(T>0){
        T = IntDiv>>Shift;
        if((T>=1)&&(T<=8)) {
            PPRE = i;
            SPRE = (~((BYTE)T-1))&0x07;
            break;
        }  
        Shift += 2; 
        i++;                  
    }    
    SPIXCON1 = (SPRE<<2)|(PPRE);

    return SPIXCON1;
}

void InitMatrix(void)
{
    AD1PCFGL = 0x0000; //set GPIO to digital

    //make pins open-collector (must be 5v tolerant!)
    //ODC_SDO = 1;
    //ODC_SCK = 1;
    //ODC_CS = 1;

    LAT_SDO = 0;
    LAT_SCK = 0;
    LAT_CS = 1;

    TRIS_SDO = 0;
    TRIS_SCK = 0;
    TRIS_CS = 0;

    //turn on scan timer and interrupt
    PR2 = (GetPeripheralClock()/(8*150*7)); //set period register
    T2CONbits.TCS = 0; //INTOSC as source
    T2CONbits.TCKPS = 1; //1:8 prescale
    _T2IP = 2;	// Interrupt priority 2 (low)
    _T2IF = 0; //clear interrupt flag
    _T2IE = 1; //TIMER2 interrupt enabled
    T2CONbits.TON = 1; //timer ON

    matrixNeedsService = 0;
    matrixScanIndex = 0;

    memset(bitmap, 0x00 /*0xFF*/, sizeof(bitmap));

};

/*****************************************************************************
  Function:
	void ConfigureSpiMRF24WB0M(void)

  Summary:
	Configures the SPI interface to the MRF24WB0M.

  Description:
	Configures the SPI interface for communications with the MRF24WB0M.

  Precondition:
	None

  Parameters:
	None

  Returns:
  	None
  	
  Remarks:
	If the SPI bus is shared with other peripherals this function is called
	each time an SPI transaction occurs by WF_SpiEnableChipSelect.  Otherwise it 
	is called once during initialization by WF_SpiInit. 
*****************************************************************************/
static void ConfigureSpiMRF24WB0M(void)
{
    /*----------------------------------------------------------------*/
    /* After we save context, configure SPI for MRF24WB0M communications */
    /*----------------------------------------------------------------*/
    /* enable the SPI clocks            */
    /* set as master                    */
    /* clock idles high                 */
    /* ms bit first                     */
    /* 8 bit tranfer length             */
    /* data changes on falling edge     */
    /* data is sampled on rising edge   */
    /* set the clock divider            */
    #if defined(__18CXX)
        WF_SPICON1 = 0x30;      // SSPEN bit is set, SPI in master mode, (0x30 is for FOSC/4),
                                //   IDLE state is high level (0x32 is for FOSC/64)
        WF_SPISTATbits.CKE = 0; // Transmit data on falling edge of clock
        WF_SPISTATbits.SMP = 1; // Input sampled at end? of data output time
    #elif defined(__C30__)
        WF_SPICON1 = 0x027B;    // Fcy Primary prescaler 1:1, secondary prescaler 2:1, CKP=1, CKE=0, SMP=1
        WF_SPICON2 = 0x0000;
        WF_SPISTAT = 0x8000;    // Enable the module
    #elif defined( __PIC32MX__ )
        WF_SPI_BRG = (GetPeripheralClock()-1ul)/2ul/WF_MAX_SPI_FREQ;
        WF_SPICON1 = 0x00000260;    // sample at end, data change idle to active, clock idle high, master
        WF_SPICON1bits.ON = 1;
    #else
        #error Configure SPI for the selected processor
    #endif
}    

void IRrecv_enableIRIn(u8 recvpin)
{
    u32 f=GetPeripheralClock();
    
    // Configure interrupt
    IntConfigureSystem(INT_SYSTEM_CONFIG_MULT_VECTOR);
    IntSetVectorPriority(INT_TIMER3_VECTOR, 7, 3);
    IntClearFlag(INT_TIMER3);
    IntEnable(INT_TIMER3);

    // Configure Timer3 to overload every 50us
    T3CON    = 0;                       // no prescaler
    TMR3     = 0;                       // clear timer register
    PR3      = 50*(f/1000/1000);        // nb cycles / 50 us
    T3CONSET = 0x8000;                  // start timer 1

    // initialize state machine variables
    irparams.recvpin = recvpin;
    irparams.blinkflag = 0;
    irparams.rcvstate = STATE_IDLE;
    irparams.rawlen = 0;

    // set pin modes
    pinmode(irparams.recvpin, INPUT);
}

void ConfigureCodec()
{
    I2CConfigure( EEPROM_I2C_BUS, 0 );
    I2CSetFrequency( EEPROM_I2C_BUS, GetPeripheralClock(), I2C_CLOCK_FREQ );
    I2CEnable( EEPROM_I2C_BUS, TRUE );
    //
    UINT8               i2cData[10];
    I2C_7_BIT_ADDRESS   SlaveAddress;

    // Initialize the data buffer
    I2C_FORMAT_7_BIT_ADDRESS(SlaveAddress, 0x46, I2C_WRITE);
    i2cData[0] = SlaveAddress.byte;
    i2cData[1] = 0x40;              // register 64
    i2cData[2] = 0xC0;              // turn off power save

    SendPacket( i2cData, 3 );

    i2cData[1] = 73;                // register 73
    i2cData[2] = 0x0C;              // inverted phase, no HPF

    SendPacket( i2cData, 3 );

    //
    I2CEnable( EEPROM_I2C_BUS, FALSE );
}

//******************************************************************************
//Public Function Definitions
//******************************************************************************
void FIFOUART1_initialize()
{
    UARTConfigure(UART1, UART_ENABLE_PINS_TX_RX_ONLY);

    UARTSetLineControl(UART1, UART_DATA_SIZE_8_BITS | //Sets the data transfer size to 8-bits per frame.�
            UART_PARITY_NONE | //Disables parity bit generation.�
            UART_STOP_BITS_1); //1 stop bit per frame (default).�

    UARTSetDataRate(UART1, GetPeripheralClock(), FIFOUART1_BAUD_RATE);



    //Interrupt Stuff
    INTSetVectorPriority(INT_UART_1_VECTOR, INT_PRIORITY_LEVEL_5);
    INTSetVectorSubPriority(INT_UART_1_VECTOR, INT_SUB_PRIORITY_LEVEL_0);

    INTClearFlag(INT_U1RX);
    INTClearFlag(INT_U1TX);

    //configure what triggers UART1 itnerrupts
    UARTSetFifoMode(UART1,
        UART_INTERRUPT_ON_TX_BUFFER_EMPTY | //TX interrupt will occur when the TX buffer is empty.�
        UART_INTERRUPT_ON_RX_NOT_EMPTY); //RX interrupt will occur whenever the RX buffer has any data.�

    //Enable UART1 Rx Interrupt
    INTEnable(INT_U1RX, INT_ENABLED);
    //Enable UART1 Tx Interrupt
    //INTEnable(INT_U1TX, INT_ENABLED);

    UARTEnable(UART1, UART_ENABLE_FLAGS(UART_PERIPHERAL | UART_RX | UART_TX));

}

BOOL MPU6050::I2CInit()
{
    INT32 actualClock;
    BOOL Success = TRUE;

    // Added but not tested yet. This is not included in example from Microchip.
    //However it is included in the following example:
    //https://gobotronics.wordpress.com/2010/12/09/i2c-eeprom-pic32/
    //I2CConfigure(this->i2cBusId, I2C_ENABLE_SLAVE_CLOCK_STRETCHING | I2C_ENABLE_HIGH_SPEED);
    I2CConfigure(this->i2cBusId, I2C_ENABLE_HIGH_SPEED);
    
    // Set the I2C baudrate
    actualClock = I2CSetFrequency( this->i2cBusId, GetPeripheralClock(), I2C_CLOCK_FREQ );
    if ( abs(actualClock-I2C_CLOCK_FREQ) > I2C_CLOCK_FREQ/10 )
    {
        //DBPRINTF("Error: I2C1 clock frequency (%u) error exceeds 10%%.\n", (unsigned)actualClock);
        sprintf(filename, "Error: I2C1 clock frequency (%u) error exceeds 10%%.\n", (unsigned)actualClock );
        Success = FALSE;
    }
    else
    {
        sprintf(filename, "I2CInit(): I2C1 clock frequency OK.\n");
    }

    // this function locks the application! Why??!!
    //putsUART1( filename );

    // Enable the I2C bus
    I2CEnable( this->i2cBusId, TRUE );

    return Success;
}

u32 SPI_setClock(u8 module, u32 Fspi)
{
    u32 Fpb = GetPeripheralClock();
    
    if (Fspi > (Fpb / 2))
    {
        SPI[module].divider = 0;        // use the maximum baud rate possible
        return (Fpb / 2);               // and return the real speed
    }
    else
    {
        // divider baudrate
        SPI[module].divider = (Fpb / (2 * Fspi)) - 1;
        
        if (SPI[module].divider > 511)
        {
            SPI[module].divider = 511;  // use the minimum baud rate possible
            return (Fpb / 1024);        // and return the real speed
        }
        else                            // ** fix for bug identified by dk=KiloOne
        {
            // return the real speed
            return ( Fpb / ( 2 * SPI[module].divider + 1));
        }
    }

    //SPI_begin();
}

//******************************************************
// void OpenUart ( const UARTx,long BaudRate)
// Overture  d'un port serie
// @param     : UARTx : choix du port 
//                UART1,UART2,UART3,UART4,UART5,UART6
//              BaudeRate : Vitesse du port serie
//                1200,2400,4800,9600,19200,38400,57600,115200
//                
//******************************************************
void OpenUart ( const UARTx,long BaudRate)
{
    UARTConfigure(UARTx, UART_ENABLE_PINS_TX_RX_ONLY);
    UARTSetFifoMode(UARTx, UART_INTERRUPT_ON_TX_NOT_FULL | UART_INTERRUPT_ON_RX_NOT_EMPTY);
    UARTSetLineControl(UARTx, UART_DATA_SIZE_8_BITS | UART_PARITY_NONE | UART_STOP_BITS_1);
    UARTSetDataRate(UARTx, GetPeripheralClock(), BAUDERATE);
    UARTEnable(UARTx, UART_ENABLE_FLAGS(UART_PERIPHERAL | UART_RX | UART_TX));
}

void initUART(void) {
    __XC_UART = 1;
    OpenUART1( UART_EN | UART_NO_PAR_8BIT | UART_1STOPBIT | UART_BRGH_SIXTEEN,
               UART_RX_ENABLE | UART_TX_ENABLE,
               GetPeripheralClock()/16/BAUD_RATE - 1);
    while( BusyUART1()); // Wait until the UART module is free.
    putsUART1("Initializing UART1...\n\r");
}

u32 OnTimer1(callback func, u32 timediv, u32 delay)
{
    u32 tckps=0, freqhz, period;

    if (intUsed[INT_TIMER1] == INT_NOT_USED)
    {
        intUsed[INT_TIMER1] = INT_USED;
        intFunction[INT_TIMER1] = func;
        intCount[1] = 0;
        intCountLimit[1] = delay;

        // TMR1 Count register increments on every PBCLK clock cycle
        freqhz = GetPeripheralClock();
    
        // Freq (Hz) = Nb ticks/sec.
        switch(timediv)
        {
            case INT_SEC:      period = freqhz;           break;
            case INT_MILLISEC: period = freqhz / 1000;    break;
            case INT_MICROSEC: period = freqhz / 1000000; break;
        }

        // Timer1 period is 16-bit, only 4 prescaler values
        while ((period > 0xFFFF) & (tckps < 5))
        {
            tckps += 1;
            period /= prescaler1[tckps];
        }

        if (tckps == 4)
        {
            tckps = 3; // divided per 256
            intCountLimit[1] = delay * 8;
        }
        
        // Configure interrupt
        IntConfigureSystem(INT_SYSTEM_CONFIG_MULT_VECTOR);
        IntSetVectorPriority(INT_TIMER1_VECTOR, 7, 3);
        IntClearFlag(INT_TIMER1);
        IntEnable(INT_TIMER1);

        // Configure Timer1
        T1CON    = tckps << 4;              // set prescaler (bit 5-4)
        TMR1     = 0;                       // clear timer register
        PR1      = period;                  // load period register
        T1CONSET = 0x8000;                  // start timer 1

        return INT_TIMER1;
    }
    
    else
    {
        #ifdef DEBUG
        debug("Error : TIMER1 interrupt is already used !");
        #endif
        return false;
    }
}

void initSD(void)
{
	digitalwrite(SDCS, HIGH);	// initially keep the SD card disabled
	pinmode(SDCS, OUTPUT);		// make Card select an output pin

	// init the spi module for a slow (safe) clock speed first
	SPI2CON = 0x8120;   // ON (0x8000), CKE=1 (0x100), CKP=0, Master mode (0x20)
	SPI2BRG = (GetPeripheralClock() / (2 * 250000)) - 1;
}   // initSD

void I2C_Initialize( I2C_MODULE id, UINT32 i2cClock, UINT16 address )
{
        // Configure Various I2C Options
        I2CConfigure(id, I2C_ENABLE_SLAVE_CLOCK_STRETCHING | I2C_ENABLE_HIGH_SPEED );
        
        // Set Desired Operation Frequency
        UINT32 actualClock = I2CSetFrequency(id, GetPeripheralClock(), i2cClock);
        if ( abs(actualClock-i2cClock) > i2cClock/10 )
        {
            DEBUG_FUNK("  Clock frequency (%d) error exceeds 10\%\r\n", actualClock);
        }

void timer_init(void)
{
    IEC0SET = _IEC0_T1IE_MASK; // enable T1 interrupt
    IFS0CLR = _IFS0_T1IF_MASK; // clear interrupt flag

    IPC1bits.T1IP = 2;

    //--- set interval 1ms timer
    T1CON = 0x0010;	// timer(T1), set prescale 1:8
    TMR1 = 0;
    PR1 = GetPeripheralClock()/8/1000L;	// period 1msec
    T1CONSET = _T1CON_TON_MASK;	// start T1
}