//find out, if message matches entry in CO_RXCAN_ID array and remember index
   _asm
      //move CO_RXCAN_ID address to FSR0
      LFSR     0, CO_RXCAN_ID
      //do{
   CO_IH_PF1:
       //if(*(FSR0++) == RXB0SIDL){
      MOVF     POSTINC0, 0, ACCESS
      SUBWF    RXB0SIDL, 0, ACCESS
      BNZ      CO_IH_PF2
        //if(*FSR0 == RXB0SIDH){
      MOVF     INDF0, 0, ACCESS
      SUBWF    RXB0SIDH, 0, ACCESS
      BNZ      CO_IH_PF2
         //break
      BRA      CO_IH_PF3
        //}
       //}
   CO_IH_PF2:
       //FSR0++; CO_IsrHighIndex++;
      INCF     FSR0L, 1, ACCESS
      INCF     CO_IsrHighIndex, 1, ACCESS
      //}while(CO_IsrHighIndex != (3+CO_NO_RPDO+CO_NO_CONS_HEARTBEAT));
      MOVLW    3+CO_NO_RPDO+CO_NO_CONS_HEARTBEAT
      SUBWF    CO_IsrHighIndex, 0, ACCESS
      BNZ      CO_IH_PF1
   CO_IH_PF3:
   _endasm
   
   if(CO_IsrHighIndex < (3+CO_NO_RPDO+CO_NO_CONS_HEARTBEAT)){//matched message
      CO_IsrHighRxNoOfBytes = RXB0DLC & 0x0F;

      /* NMT message from master */
      if(CO_IsrHighIndex == 0){
         if(CO_IsrHighRxNoOfBytes != 2) CO_IsrHighSignal.Error.bits.NMTlength = 1;
         else if(RXB0D1 == 0 || RXB0D1 == CO_NodeID){
            switch(RXB0D0){
               case NMT_ENTER_OPERATIONAL:      CO_NMToperatingState = NMT_OPERATIONAL; break;
               case NMT_ENTER_STOPPED:          CO_NMToperatingState = NMT_STOPPED; break;
               case NMT_ENTER_PRE_OPERATIONAL:  CO_NMToperatingState = NMT_PRE_OPERATIONAL; break;
               case NMT_RESET_NODE:             Reset(); break;  //reset node
               case NMT_RESET_COMMUNICATION:    CO_IsrHighSignal.ResetComm = 1; break; //reset communication
               default:  CO_IsrHighSignal.Error.bits.NMTcmd = 1;
            }
         }
      }
      
      else if(CO_NMToperatingState==NMT_PRE_OPERATIONAL || CO_NMToperatingState==NMT_OPERATIONAL){      
         /* SYNC messaage */
         if(CO_IsrHighIndex == 1){
            if(CO_IsrHighRxNoOfBytes != 0) CO_IsrHighSignal.Error.bits.SyncLength = 1;
            else{//valid sync recived
               ODE_SYNCcounter++;
               ODE_SYNCtime = 0;
            }
         }
            
         /* SDO communication */
         else if(CO_IsrHighIndex == 2){
            if(CO_IsrHighRxNoOfBytes != 8) CO_IsrHighSignal.Error.bits.SDOlength = 1;
            else if(CO_SDOrequest == 0){//check if previous SDO was processed, otherwise ignore
               //copy CAN data to SDO data
               //memcpy((void*)CO_SDO.data, (void*)&RXB0D0, 8);
               _asm
                  LFSR     0, CO_SDORXdata
                  MOVFF    RXB0D0, POSTINC0
                  MOVFF    RXB0D1, POSTINC0
                  MOVFF    RXB0D2, POSTINC0
                  MOVFF    RXB0D3, POSTINC0
                  MOVFF    RXB0D4, POSTINC0
                  MOVFF    RXB0D5, POSTINC0
                  MOVFF    RXB0D6, POSTINC0
                  MOVFF    RXB0D7, POSTINC0
               _endasm
               CO_SDOrequest = 1;
            }
         }
            
         #if CO_NO_RPDO > 0
         /* Recived PDOs communication */
         else if(CO_IsrHighIndex < (3+CO_NO_RPDO)){
            if(CO_NMToperatingState==NMT_OPERATIONAL){
               CO_IsrHighIndex -= 3;
               if(CO_IsrHighRxNoOfBytes != CO_RPDOlength[CO_IsrHighIndex]) CO_IsrHighSignal.Error.bits.PDOlength = 1;
               else if(CO_IsrHighRxNoOfBytes){
                  CO_RPDOcount[CO_IsrHighIndex]++;
                  //copy CAN data to RPDO data
                  //memcpy((void*)&CO_RPDO[CO_IsrHighIndex], (void*)&RXB0D0, CO_IsrHighRxNoOfBytes);
                  _asm
                     LFSR     0, CO_RPDO
                     MOVF     FSR0L, 0, ACCESS

                  CO_IH_RPDO1:
                     DECF     CO_IsrHighIndex, 1, ACCESS
                     BNC      CO_IH_RPDO2
                     ADDLW    CO_SIZE_OF_RPDO_DATA
                     BRA      CO_IH_RPDO1
                     
                  CO_IH_RPDO2:
                     MOVWF    FSR0L, ACCESS
                     MOVFF    RXB0D0, POSTINC0
                     DCFSNZ   CO_IsrHighRxNoOfBytes, 1, ACCESS
                     BRA      CO_IH_RPDO3
                     MOVFF    RXB0D1, POSTINC0
                     DCFSNZ   CO_IsrHighRxNoOfBytes, 1, ACCESS
                     BRA      CO_IH_RPDO3
                     MOVFF    RXB0D2, POSTINC0
                     DCFSNZ   CO_IsrHighRxNoOfBytes, 1, ACCESS
                     BRA      CO_IH_RPDO3
                     MOVFF    RXB0D3, POSTINC0
                     DCFSNZ   CO_IsrHighRxNoOfBytes, 1, ACCESS
                     BRA      CO_IH_RPDO3
                     MOVFF    RXB0D4, POSTINC0
                     DCFSNZ   CO_IsrHighRxNoOfBytes, 1, ACCESS
                     BRA      CO_IH_RPDO3
                     MOVFF    RXB0D5, POSTINC0
                     DCFSNZ   CO_IsrHighRxNoOfBytes, 1, ACCESS
                     BRA      CO_IH_RPDO3
                     MOVFF    RXB0D6, POSTINC0
                     DCFSNZ   CO_IsrHighRxNoOfBytes, 1, ACCESS
                     BRA      CO_IH_RPDO3
                     MOVFF    RXB0D7, POSTINC0
                  CO_IH_RPDO3:
                  _endasm  //index and NoOfBytes are no more correct
               }
            }
         }
         #endif
            
         #if CO_NO_CONS_HEARTBEAT > 0
          /* Consumer heartbeat message */
          else{
             if(CO_IsrHighRxNoOfBytes != 1) CO_IsrHighSignal.Error.bits.HeartBeatLength = 1;
             else if(RXB0D0){ //ignore bootup message
                CO_IsrHighIndex -= (3+CO_NO_RPDO);
                CO_HBcons_TimerValue[CO_IsrHighIndex] = CO_Timer16Value;  //reset timer
                CO_HBcons_NMTstate[CO_IsrHighIndex] = RXB0D0;
             }
          }
         #endif
            
      }
         
   }
   RXB0CONbits.RXFUL = 0;  //release buffer
   PCB_BANDWIDTH_IsrHigh(0);   
}

/*******************************************************************************
   INTERRUPT SERVICE ROUTINE
   
   Transmition of messages:
      There are three transmit buffers, all are interrupt driven. First is used 
      for transmitting SDOs, Heartbeats and user messages, second is for PDOs, 
      third for Sync and Emergencies. These messages has own buffer. When CAN transmit
      registers are free, interrupt occures and copies message to CAN registers.
      
*******************************************************************************/
void CO_IsrCANtxErr(void){
   static char i;

   /***** Transmition of SYNC and Emergency ***********************************/
   if(PIE3bits.TXB2IE && PIR3bits.TXB2IF){
      if(CO_TXCAN_SEMsendReq & 1){
         CO_CanSend(CO_TXCAN_SEMmsgs[0], 2, 3);
         CO_TXCAN_SEMsendReq &= 0xFE;
      }
      else if(CO_TXCAN_SEMsendReq & 2){
         CO_CanSend(CO_TXCAN_SEMmsgs[1], 2, 3);
         CO_TXCAN_SEMsendReq &= 0xFD;
      }
      else //disable interrupt if buffers are empty
         PIE3bits.TXB2IE = 0; 
   }

   /***** Transmition of PDOs *************************************************/
   #if CO_NO_TPDO > 0
   else if(PIE3bits.TXB1IE && PIR3bits.TXB1IF){
      //check if there are any data in buffers to send
      if(CO_TXCAN_PDOsendReq){
         unsigned char SendReq = CO_TXCAN_PDOsendReq;
         unsigned int SYNCtimeCopy;
         INTCONbits.GIEH = 0;
         SYNCtimeCopy = ODE_SYNCtime;
         INTCONbits.GIEH = 1;
   
         for(i=0; i<CO_NO_TPDO; i++){
            if(SendReq & 1){
               //check if synchronous PDOs are transmitted inside preset window
               if(CO_SYNC.window && SYNCtimeCopy > CO_SYNC.window && 
                  (ODE_TPDO_Parameter[i].Transmission_type-1) <= 239)
                  ErrorReport(ERROR_TPDO_OUTSIDE_WINDOW, SYNCtimeCopy);
               else 
                  CO_CanSend(CO_TXCAN_PDOmsgs[i], 0, 2);
               break;
            }
            SendReq >>= 1;
         }
         //clear flag
         CO_TXCAN_PDOsendReq &= (1 << i) ^ 0xFF;
      }
      else //disable interrupt if buffers are empty
         PIE3bits.TXB1IE = 0; 
   }
   #endif

   /***** Transmition of other messages ***************************************/
   else if(PIE3bits.TXB0IE && PIR3bits.TXB0IF){
      //check if there are any data in buffers to send
      if(CO_TXCAN_OtherSendReq){
         unsigned char SendReq = CO_TXCAN_OtherSendReq;
         for(i=0; i<CO_NO_USR_CAN_BUFF+2; i++){
            if(SendReq & 1){
               CO_CanSend(CO_TXCAN_OtherMsgs[i], 1, 1);
               break;
            }
            SendReq >>= 1;
         }
         //clear flag
         CO_TXCAN_OtherSendReq &= (1 << i) ^ 0xFF;
      }
      else //disable interrupt if buffers are empty
         PIE3bits.TXB0IE = 0; 
   }

   /***** Errors **************************************************************/
   //ERRIF is set on every change of COMSTAT, except on change to zero
   else if(PIR3bits.ERRIF && PIE3bits.ERRIE){
      if(COMSTAT & 0xC0){
         ErrorReport(ERROR_CAN_RXB_OVERFLOW, COMSTAT);
         COMSTAT &= 0x3F;   //RXBnOVFL bits must be cleared or ERRIF can not be reset
      }
      if(COMSTATbits.TXBO)
         ErrorReport(ERROR_CAN_TX_BUS_OFF, COMSTAT);
      if(COMSTATbits.TXBP)
         ErrorReport(ERROR_CAN_TX_BUS_PASSIVE, COMSTAT);
      if(COMSTATbits.RXBP)
         ErrorReport(ERROR_CAN_RX_BUS_PASSIVE, COMSTAT);
      if(COMSTAT & 0x07)
         ErrorReport(ERROR_CAN_BUS_WARNING, COMSTAT);
      PIR3bits.ERRIF = 0;
   }
}

/*******************************************************************************
   Process CANopen from 1ms interrupt
   Used for reliable, accurate and fast procedures
*******************************************************************************/
void CO_IsrProcess1ms(void){
   static unsigned char i;
    
   INTCONbits.GIEH = 0;
   CO_Timer16Value++;
   INTCONbits.GIEH = 1;

/* Dealing with SYNC **********************************************************/
   if(CO_SYNC.period){//SYNC is enabled
      unsigned int SYNCtimeCopy;
      INTCONbits.GIEH = 0;
      SYNCtimeCopy = ODE_SYNCtime;
      INTCONbits.GIEH = 1;
   
      //if SYNC is recived or transmitted send SYNC PDOs
      #if CO_NO_TPDO > 0
         if(SYNCtimeCopy == 0 && CO_NMToperatingState==NMT_OPERATIONAL){
            for(i=0; i<CO_NO_TPDO; i++){
               if((romBYTE3(ODE_TPDO_Parameter[i].COB_ID) & 0x80) == 0 && //is TPDO used
                  CO_TPDOlength[i] && --CO_tTPDOwait[i] == 0){
                  if((ODE_TPDO_Parameter[i].Transmission_type-1) <= 239){//is value from 1...240
                     unsigned char flag = 1 << i;
                     CO_tTPDOwait[i] = ODE_TPDO_Parameter[i].Transmission_type;
                     if(CO_TXCAN_PDOsendReq & flag)
                        ErrorReport(ERROR_CanSendPDO_Overflow, i);
                     else{
                        CO_TXCAN_PDOsendReq |= flag;
                        PIE3bits.TXB1IE = 1; //inform ISR
                     }
                  }
                  else CO_tTPDOwait[i] = 254;
               }
            }
         }
      #endif
      
      //increment timers
      INTCONbits.GIEH = 0;
      ODE_SYNCtime++;
      INTCONbits.GIEH = 1;
      
      //SYNC producer
         //SYNC message can be recived or transmited, not both in same time. 
         //Useful variables are ODE_SYNCcounter and ODE_SYNCtime, which define 
         //exact time in [ms] synchronised in all nodes. 
      if((romBYTE3(ODE_SYNC_COB_ID) & 0x40) &&
        (CO_NMToperatingState==NMT_PRE_OPERATIONAL || CO_NMToperatingState==NMT_OPERATIONAL)){
         static unsigned int tSYNCperiod = 0;
         if(++tSYNCperiod >= CO_SYNC.period){
            CO_TXCAN_SEMsendReq |= 1;
            PIE3bits.TXB2IE = 1; //inform ISR
            INTCONbits.GIEH = 0;
            ODE_SYNCcounter++;
            ODE_SYNCtime = 0;
            INTCONbits.GIEH = 1;
            tSYNCperiod = 0;
         }
      }
      
      //presence of sync pulse
      if((SYNCtimeCopy>CO_SYNC.periodTimeout) && CO_NMToperatingState==NMT_OPERATIONAL)