extern hal_result_t s_hal_eeprom_dspic_emuflash_erase(_prog_addressT EE_address, int16_t size)
{
    _prog_addressT addr = EE_address;
    int16_t remaining_data =  size;
     
    #warning -> acemor: maybe a check upon validity of the address and size ??
    
    do
    {
        
        if(remaining_data < BYTES_PER_PAGE)
        {
            _erase_flash(addr);
            remaining_data = 0;
        }
        else
        {
             _erase_flash(addr);
            remaining_data -= BYTES_PER_PAGE;
            addr += PCUNIT_PER_PAGE;
        }
    }
    while(remaining_data > 0);   

    return(hal_res_OK);
}
BOOL FWNewEnable_GprsPro()
{
	BOOL enable_low = FALSE, enable_high = FALSE;
	BYTE i,fread[1];
	//	Check on low word of reset vector
	for (i = 0; i < 3;i++)
	{
		vTaskSuspendAll();
		SPIFlashReadArray(FLASH_START_ADD + i, fread, 1);
		xTaskResumeAll();
		if (fread[0] != 0xFF)
			enable_low = TRUE;
	}
	//	Check on high word of reset vector
	for (i = 3; i < 6;i++)
	{
		vTaskSuspendAll();
		SPIFlashReadArray(FLASH_START_ADD + i, fread, 1);
		xTaskResumeAll();
		if (fread[0] != 0xFF)
			enable_high = TRUE;
	}
	if ( (enable_low == TRUE) && (enable_high == TRUE) )
	{
		vTaskSuspendAll();
		_erase_flash(0x29800);
		xTaskResumeAll();
		return TRUE;
	}
	else
		return FALSE;
}
static void s_hal_eeprom_dspic_emuflash_write_1_page(_prog_addressT EE_address, int16_t size, void *buf)
{

    int16_t buf_flash_pag1[_FLASH_PAGE]; //Note: this buffer contains 1024 bytes = num of bytes in a flash page
    _prog_addressT addr_row;
    int16_t *buf_row;
    int16_t i;

    /*
        function for reading and writing from/to flash with 16 bits read and write 2 bytes for each instruction.
        One instruction = 2 PC units ==> 4 bytes : 1 phantom, 3 real.
        if you use function about 16 bits you use only 2 bytes of 3 real bytes
        else if use function about 24 bits you use 3 bytes of 3 real bytes.

        /--INSTRUCTION--/
        |---|---|---|---|
        | P | 1 | 2 | 3 |
        |___|___|___|___|
        /------//-------/
        1PCunit  1PCunit

        P = phantom byte
        1,2,3 = real bytes used in binary code and in ...d24 flash instruction
        2,3 = bytes used by ...d16 flash instructions (byte 3 is not used)
    
    */
 
    /* The third parameter in this case (function ...d16) express the number
     * of bytes to be written. Note that since we write 2 bytes per instruction 
     * (see above explanation) we are actually writing 2 bytes for each 2 PC units 
     * (because 1 instraction = 2 p.c. units) SO you can think about the 3rd parameter
     * also as P.C. units. In other words in this case (..d16 instruction) the number
     * of PC units for 3rd param is equal to the nuber of bytes.
     * !!!!!!! The important thing is that this has to be checked for
     * ...d24 functions!!!!!!!!
     */
    _memcpy_p2d16(buf_flash_pag1, EE_address, BYTES_PER_PAGE);


    _erase_flash(EE_address); //a flash page is the smallest program mem erase unit

    memcpy((int8_t*)buf_flash_pag1,(int8_t*)(buf), size);

    // a row is the smollest program mem program unit (a raw = 64 instructions)
    addr_row = EE_address;
    buf_row = buf_flash_pag1;
    for(i=0; i<ROWS_PER_PAGE; i++)
    {
        _write_flash16(addr_row, buf_row);
        addr_row += PCUNIT_PER_ROW;
        buf_row += BYTES_PER_ROW/2; //divided by two because buf_row is a pointer to int16_t=2 bytes
    }
}
static int s_canIcubProtoParser_parse_pollingMsg(tCanData *rxpayload, unsigned char rxlen, tCanData *txpayload, unsigned char *txlen)
{
    unsigned char cmd = rxpayload->b[0];

    *txlen=0;
    
    switch (cmd)
    {
    case ICUBCANPROTO_POL_MC_CMD__CONTROLLER_RUN: // DS402 Operation Enable
    {
        if (rxlen!=1 || !DS402_Statusword.Flags.SwitchedOn || !sCanProtocolCompatible) return 0;

        //when start PWM ==> set the default control mode
        SysStatus.b[0] = 0;

        // go to Operation Enable state
        DS402_Controlword.Flags.EnableOperation = 1;
    }
    break;

    case ICUBCANPROTO_POL_MC_CMD__DISABLE_PWM_PAD: // DS402 Operation Disable
    {
        if (rxlen!=1) return 0;

        FaultReset();

        // Command can be accepted only if current ststus is OPERATION ENABLE
        if (!DS402_Statusword.Flags.OperationEnabled && !DS402_Statusword.Flags.SwitchedOn) return 0;

        // In ICUB the states DISABLE_OPERATION and SHUTDOWN goes in the same state.
        // go to Ready to Switch On state
        DS402_Controlword.Flags.EnableVoltage = 0;
    }
    break;

    case ICUBCANPROTO_POL_MC_CMD__ENABLE_PWM_PAD: // DS402 Switch On
    {
        if (rxlen!=1 || !DS402_Statusword.Flags.ReadyToSwitchOn || !sCanProtocolCompatible) return 0;

        received_canloader_msg = 0; // start to transmit status messages

        // go to Switch On state
        DS402_Controlword.Flags.SwitchOn = 1;
    }
    break;

    case ICUBCANPROTO_POL_MC_CMD__CONTROLLER_IDLE: // DS402 Shutdown
    {
        if (rxlen!=1 || (!DS402_Statusword.Flags.OperationEnabled && !DS402_Statusword.Flags.SwitchedOn)) return 0;

        // go to Ready to Switch On state
        //DS402_Controlword.Flags.EnableVoltage = 0;
        DS402_Controlword.Flags.EnableOperation = 0;
    }
    break;

    case ICUBCANPROTO_POL_MC_CMD__GET_CONTROL_MODE:
    {
        if (rxlen!=1) return 0;

        txpayload->b[1] = CanIcubProtoGetcontrol_mode();

        if (txpayload->b[1] == icubCanProto_controlmode_unknownError) return 0;

        *txlen = 2;
        txpayload->b[0] = cmd;
    }
    break;

    case ICUBCANPROTO_POL_MC_CMD__SET_CONTROL_MODE:
    {
        if (rxlen!=2 || !sCanProtocolCompatible) return 0;

        received_canloader_msg = 0; // start to transmit status messages

        iCubProtControlMode = rxpayload->b[1];

        CtrlReferences.IqRef = 0;
        CtrlReferences.VqRef = 0;
        CtrlReferences.WRef = 0;

        if (rxpayload->b[1] != icubCanProto_controlmode_idle)
        {
            DS402_Controlword.Flags.SwitchOn = 1;
            DS402_Controlword.Flags.EnableOperation = 1;
        }

        switch (rxpayload->b[1])
        {
        case icubCanProto_controlmode_openloop:
            SysStatus.b[0] = 0;
            break;
        case icubCanProto_controlmode_current:
            SysStatus.b[0] = 1;
            break;
        case icubCanProto_controlmode_velocity:
        case icubCanProto_controlmode_speed_voltage:
            SysStatus.b[0] = 2;
            break;
        case icubCanProto_controlmode_speed_current:
            SysStatus.b[0] = 3;
            break;
        case icubCanProto_controlmode_idle:
            FaultReset();
            if (!Fault()) LED_status.GreenBlinkRate = BLINKRATE_FAST;
            DS402_Controlword.Flags.EnableOperation = 0; // go to Switched On state
        }
    }
    break;

    case ICUBCANPROTO_POL_MC_CMD__WRITE_FLASH_MEM:
        //EepromSave();
        break;

    case ICUBCANPROTO_POL_MC_CMD__GET_ADDITIONAL_INFO:
    case ICUBCANPROTO_POL_MC_CMD__SET_ADDITIONAL_INFO:
    case ICUBCANPROTO_POL_MC_CMD__GET_DESIRED_VELOCITY:
    case ICUBCANPROTO_POL_MC_CMD__SET_DESIRED_VELOCITY:
    break;

    case ICUBCANPROTO_POL_MC_CMD__SET_ENCODER_POSITION:
    {
        // setta l'offset di fasatura
        //Encoder_SyncPulsePosition = (rxpayload->b[2] << 8 | rxpayload->b[1]); //todo aspetta maggia
    }
    break;


    case ICUBCANPROTO_POL_MC_CMD__GET_BOARD_ID:
    {
        if (rxlen!=1) return 0;
        
        *txlen=2;
        txpayload->b[0] = cmd;
        txpayload->b[1] = canprotoparser_bid;
    }
    break;

    case ICUBCANPROTO_POL_MC_CMD__SET_BOARD_ID:
    {
        if (rxlen!=2) return 0;

        #ifndef CAN_CMD_ALWAYS_ACCEPT
        if (!DS402_Statusword.Flags.OperationEnabled && !DS402_Statusword.Flags.SwitchedOn) return 0;
        #endif
        
        canprotoparser_bid = rxpayload->b[1];
        ApplicationData.EepromHeader.EE_CAN_BoardAddress = canprotoparser_bid;
        CanIcubProtoTransmitterUpdateBoardId(canprotoparser_bid);
        CanIcubProtoSetFilters(canprotoparser_bid);

        _memcpy_p2d16(&s_devinfo, 0x15000, sizeof(s_deviceinfo_in_flash_t));

        _erase_flash(0x15000);

        s_devinfo.canadr = canprotoparser_bid;

        s_devinfo.mode   = 1;
        s_devinfo.dummy0 = 0;
        s_devinfo.dummy1 = 0xcaac;
        s_devinfo.crc    = crc16(0xFFFF, (const unsigned char*)&s_devinfo, 64);
                  
        int i;
        for (i=0; i<sizeof(s_deviceinfo_in_flash_t); i+=_FLASH_ROW)
        {
            _write_flash16(0x15000+i, ((int*)(&s_devinfo))+i/2);
        }
    }
    break;

    case ICUBCANPROTO_POL_MC_CMD__SET_MAX_VELOCITY:
    case ICUBCANPROTO_POL_MC_CMD__GET_MAX_VELOCITY:
        break;

    case ICUBCANPROTO_POL_MC_CMD__SET_CURRENT_LIMIT:
    {
        if (rxlen!=5) return 0;

        ////il dato che arriva è espresso in mA va trasformato in IAD della 2FOC (1A 1310)
        //il dato che arriva è espresso in mA va trasformato in IAD della 2FOC (1A=2000)
        ApplicationData.CurLimit = (rxpayload->b[2] << 8 | rxpayload->b[1])*2;
    }
    break;

    case ICUBCANPROTO_POL_MC_CMD__GET_FIRMWARE_VERSION:
    {
        uint8_t server_can_protocol_major = rxpayload->b[1];
        uint8_t server_can_protocol_minor = rxpayload->b[2];

        sCanProtocolCompatible = (
            CAN_PROTOCOL_VERSION_MAJOR == server_can_protocol_major &&
            CAN_PROTOCOL_VERSION_MINOR == server_can_protocol_minor);

        *txlen = 0x8;
        txpayload->b[0] = ICUBCANPROTO_POL_MC_CMD__GET_FIRMWARE_VERSION;
        txpayload->b[1] = icubCanProto_boardType__2foc;
        txpayload->b[2] = FW_VERSION_MAJOR;
        txpayload->b[3] = FW_VERSION_MINOR;
        txpayload->b[4] = FW_VERSION_BUILD;
        txpayload->b[5] = CAN_PROTOCOL_VERSION_MAJOR;
        txpayload->b[6] = CAN_PROTOCOL_VERSION_MINOR;
        txpayload->b[7] = sCanProtocolCompatible; // can_protocol_ack;

        if (!sCanProtocolCompatible)
        {
            // go to fault state
            SysError.CANInvalidProtocol = 1;
            // call fault handler
            FaultConditionsHandler();
        }

    }
    break;

    case ICUBCANPROTO_POL_MC_CMD__SET_CURRENT_PID:
    {
        SFRAC16 pp, pi, pd, pm; //pm non viene passato dal comando

        if (rxlen!=7) return 0;

        //ControllerGetCurrentDPIDParm(&pp,&pi,&pd,&pm);
        pp = (rxpayload->b[2] << 8 | rxpayload->b[1]);
        pi = (rxpayload->b[4] << 8 | rxpayload->b[3]);
        pd = 0;
        pm = (rxpayload->b[6] << 8 | rxpayload->b[5]);


        //ControllerSetCurrentDPIDParm(pp, pi, pd, pm);
        //ControllerSetCurrentQPIDParm(pp, pi, pd, pm);
    }
    break;

    case ICUBCANPROTO_POL_MC_CMD__GET_CURRENT_PID:
    {
        signed int p, i, d;

        if (rxlen!=1) return 0;

        //ControllerGetCurrentDPIDParm(&p, &i, &d, &m);
        txpayload->b[0] = cmd;
        memcpy(&txpayload->b[1], &p, 2);
        memcpy(&txpayload->b[3], &i, 2);
        memcpy(&txpayload->b[5], &d, 2);
        *txlen = 7;
    }
    break;

    case ICUBCANPROTO_POL_MC_CMD__SET_VELOCITY_PID:
    {
        SFRAC16 pp, pi, pd; //pm non viene passato dal comando

        if (rxlen!=7) return 0;
        
        //ControllerGetWPIDParm(&pp, &pi, &pd, &pm);
        pp = (rxpayload->b[2] << 8 | rxpayload->b[1]);
        pi = (rxpayload->b[4] << 8 | rxpayload->b[3]);
        pd = (rxpayload->b[6] << 8 | rxpayload->b[5]);
        //ControllerSetWPIDParm(pp, pi, pd, pm);
    }
    break;

    case ICUBCANPROTO_POL_MC_CMD__GET_VELOCITY_PID:
    {
        signed int p, i, d;

        if (1 != rxlen) return (0);
        
        //ControllerGetWPIDParm(&p, &i, &d, &m);
        txpayload->b[0] = cmd;
        memcpy(&txpayload->b[1], &p, 2);
        memcpy(&txpayload->b[3], &i, 2);
        memcpy(&txpayload->b[5], &d, 2);
        *txlen = 7;
    }
    break;

    case ICUBCANPROTO_POL_MC_CMD__SET_DESIRED_CURRENT:
    {
        if (rxlen!=5) return 0;

#ifndef CAN_CMD_ALWAYS_ACCEPT
        if (!DS402_Statusword.Flags.OperationEnabled && !DS402_Statusword.Flags.SwitchedOn) return 0;
#endif
        // Torque control references
        int ctrlRef = (rxpayload->b[2] << 8 | rxpayload->b[1]);

        switch (iCubProtControlMode)
        {
        case icubCanProto_controlmode_velocity:
        case icubCanProto_controlmode_speed_voltage:
        case icubCanProto_controlmode_speed_current:
            CtrlReferences.WRef = ctrlRef;
            LIMIT(CtrlReferences.WRef, UDEF_SPEED_MAX)
            break;

        case icubCanProto_controlmode_current:
            CtrlReferences.IqRef = ctrlRef;
            LIMIT(CtrlReferences.IqRef, UDEF_CURRENT_MAX)
            break;

        case icubCanProto_controlmode_openloop:
            CtrlReferences.VqRef = ctrlRef;
            LIMIT(CtrlReferences.VqRef, UDEF_PWM_MAX)
            break;

        case icubCanProto_controlmode_idle:
            break;
        }

#ifdef SYNC_2FOC_TO_EMS
        CanIcubProtoTrasmitterSendPeriodicData();
#endif    
    }
    break;

    case ICUBCANPROTO_POL_MC_CMD__GET_DESIRED_CURRENT:
    {
        if (rxlen!=1) return 0;

        *txlen = 5;
        txpayload->b[0] = cmd;
        memcpy(&txpayload->b[1], &CtrlReferences.IqRef, 2);
        txpayload->b[3] = txpayload->b[4] = 0;
    }
    break;

    case ICUBCANPROTO_POL_MC_CMD__SET_PERIODIC_MSG_CONTENTS:
    {
        if (rxlen!=5) return 0;

        // check data to transmit if in the acceptable range
        if ((rxpayload->b[1] < ELEMENTS_IN_PREIODIC_DATA_LIST)
                && (rxpayload->b[2] < ELEMENTS_IN_PREIODIC_DATA_LIST)
                && (rxpayload->b[3] < ELEMENTS_IN_PREIODIC_DATA_LIST)
                && (rxpayload->b[4] < ELEMENTS_IN_PREIODIC_DATA_LIST))
        {
            // set the data to transmit
            PeriodicMessageContents[0] = rxpayload->b[1];
            PeriodicMessageContents[1] = rxpayload->b[2];
            PeriodicMessageContents[2] = rxpayload->b[3];
            PeriodicMessageContents[3] = rxpayload->b[4];

            // precalculate this here. This is an optimization
            gulpadr1 = (unsigned int*) PeriodicData[PeriodicMessageContents[0]];
            gulpadr2 = (unsigned int*) PeriodicData[PeriodicMessageContents[1]];
            gulpadr3 = (unsigned int*) PeriodicData[PeriodicMessageContents[2]];
            gulpadr4 = (unsigned int*) PeriodicData[PeriodicMessageContents[3]];
        }
        else
        {
            return 0;
        }
    }
    break;

    case ICUBCANPROTO_POL_MC_CMD__SET_I2T_PARAMS:
    {
        if (rxlen!=5) return 0;
     
        //extract params from the 1st word
        I2Tdata.Param = (rxpayload->b[2] << 8 | rxpayload->b[1]);
        I2Tdata.IThreshold = (rxpayload->b[4] << 8 | rxpayload->b[3]);
    }
    break;

    case ICUBCANPROTO_POL_MC_CMD__GET_I2T_PARAMS:
    {
        if (rxlen!=1) return 0;
 
        *txlen = 5;
        txpayload->b[0] = cmd;
        memcpy(&txpayload->b[1], &I2Tdata.Param, 2);
        memcpy(&txpayload->b[3], &I2Tdata.IThreshold, 2);
    }
    break;

    default:
        return 0;
    } //end switch
    return 1;
}