canopdriver.c

Pic18Fxx8单片机下的canopen协议源代码

   //PDOs
   #if CO_NO_RPDO > 0
      for(i=0; i<CO_NO_RPDO; i++){
         char j;
         CO_RPDO[i].DWORD[0] = 0;
         CO_RPDO[i].DWORD[1] = 0;
         CO_RPDOlength[i] = 7;  //round up to use whole byte
         for(j=0; j<CO_PDO_MAP_SIZE; j++)
            CO_RPDOlength[i] += (unsigned char)ODE_RPDO_Mapping[i].Map[j];
         CO_RPDOlength[i] >>= 3;  
         if(CO_RPDOlength[i] > 8){
            CO_RPDOlength[i] = 8; 
            ErrorReport(ERROR_wrong_PDO_mapping, i);
         }
      }
   #endif
   #if CO_NO_TPDO > 0
      for(i=0; i<CO_NO_TPDO; i++){
         char j;
         CO_TPDO[i].DWORD[0] = 0;
         CO_TPDO[i].DWORD[1] = 0;
         CO_tTPDOwait[i] = 1;
         CO_TPDOlength[i] = 7;  //round up to use whole byte
         for(j=0; j<CO_PDO_MAP_SIZE; j++)
            CO_TPDOlength[i] += (unsigned char)ODE_TPDO_Mapping[i].Map[j];
         CO_TPDOlength[i] >>= 3;   
         if(CO_TPDOlength[i] > 8){
            CO_TPDOlength[i] = 8;
            ErrorReport(ERROR_wrong_PDO_mapping, i);
         }
      }
   #endif

   //Sync
   CO_SYNC.period = (unsigned int)(ODE_Communication_Cycle_Period / 1000);
   CO_SYNC.periodTimeout = (unsigned int)((ODE_Communication_Cycle_Period * 3) / 2000);//1,5 * CO_SYNC.period
   CO_SYNC.window = (unsigned int)(ODE_Synchronous_Window_Length / 1000);
   ODE_SYNCcounter = 0;
   ODE_SYNCtime = 1;
      
   //HeartBeat Consumer
   #if CO_NO_CONS_HEARTBEAT > 0
      for(i=0; i<CO_NO_CONS_HEARTBEAT; i++)
         CO_HBcons_NMTstate[i] = NMT_INTIALIZING;
   #endif

   //Errors and Emergency
   ErrorControl.EmergencyToSend = 0;
   ErrorControl.CheckErrors = 1;
   for(i=0; i<ODE_ErrorStatusBitsNoOfEntries; i++)
      ODE_ErrorStatusBits[i] = 0;
   ODE_Error_Register = 0;
   #if CO_NO_ERROR_FIELD > 0
     ODE_Pre_Defined_Error_Field_NoOfErrors = 0;
   #endif
   ODE_Emergency_COB_ID = 0x00000080L + CO_NodeID;

   //fill CO_RXCAN data
   CO_RXCAN_ID[0] = 0;
   CO_RXCAN_ID[1] = (unsigned int)ODE_SYNC_COB_ID << 5;
   CO_RXCAN_ID[2] = (unsigned int)ODE_Server_SDO_Parameter.COB_ID_Client_to_Server << 5;
   #if CO_NO_RPDO > 0
      for(i=3; i<3+CO_NO_RPDO; i++){
         if((romBYTE3(ODE_RPDO_Parameter[i-3].COB_ID) & 0x80) == 0)//is RPDO used       
            CO_RXCAN_ID[i] = (unsigned int)ODE_RPDO_Parameter[i-3].COB_ID << 5;
         else CO_RXCAN_ID[i] = 0;
      }
   #endif
   #if CO_NO_CONS_HEARTBEAT > 0
      for(i; i<3+CO_NO_RPDO+CO_NO_CONS_HEARTBEAT; i++){
         if(romBYTE2(ODE_Consumer_Heartbeat_Time[i-(3+CO_NO_RPDO)]))
            CO_RXCAN_ID[i] = (romBYTE2(ODE_Consumer_Heartbeat_Time[i-(3+CO_NO_RPDO)]) | CAN_ID_HEARTBEAT) << 5;
         else CO_RXCAN_ID[i] = 0;
    }
   #endif

   //fill CO_TXCAN data
   #if CO_NO_TPDO > 0
      CO_TXCAN_PDOsendReq = 0;
      for(i=0; i<CO_NO_TPDO; i++){
         CO_TXCAN_PDOmsgs[i].COB_ID_LSH5 = ((unsigned int)ODE_TPDO_Parameter[i].COB_ID) << 5;
         CO_TXCAN_PDOmsgs[i].NoOfBytes = CO_TPDOlength[i];
         CO_TXCAN_PDOmsgs[i].pData = (unsigned char *)&CO_TPDO[i];
         }
   #endif
   
   CO_TXCAN_OtherSendReq = 0;
   CO_TXCAN_OtherMsgs[CO_NO_USR_CAN_BUFF].COB_ID_LSH5 = ((unsigned int)ODE_Server_SDO_Parameter.COB_ID_Server_to_Client) << 5;
   CO_TXCAN_OtherMsgs[CO_NO_USR_CAN_BUFF].NoOfBytes = 8;
   CO_TXCAN_OtherMsgs[CO_NO_USR_CAN_BUFF].pData = CO_SDOTXdata;
   CO_TXCAN_OtherMsgs[CO_NO_USR_CAN_BUFF+1].COB_ID_LSH5 = (CAN_ID_HEARTBEAT + CO_NodeID) << 5;
   CO_TXCAN_OtherMsgs[CO_NO_USR_CAN_BUFF+1].NoOfBytes = 1;
   CO_TXCAN_OtherMsgs[CO_NO_USR_CAN_BUFF+1].pData = &CO_NMToperatingState;

   CO_TXCAN_SEMmsgs[0].COB_ID_LSH5 = ((unsigned int)ODE_SYNC_COB_ID) << 5;
   CO_TXCAN_SEMmsgs[0].NoOfBytes = 0;
   CO_TXCAN_SEMmsgs[1].COB_ID_LSH5 = ((unsigned int)ODE_Emergency_COB_ID) << 5;
   CO_TXCAN_SEMmsgs[1].NoOfBytes = 8;
   CO_TXCAN_SEMmsgs[1].pData = CO_EmergencyTXdata;

/* User function **************************************************************/
   User_InitResetComm();
   
   
/* Setup CAN bus **************************************************************/
   BRGCON1 = ((CO_BitRateData[CO_BitRate].SJW-1) <<6 ) | (CO_BitRateData[CO_BitRate].BRP-1);
   BRGCON2 = 0x80 | ((CO_BitRateData[CO_BitRate].PhSeg1-1)<<3) | (CO_BitRateData[CO_BitRate].PROP-1);
   BRGCON3 = (CO_BitRateData[CO_BitRate].PhSeg2-1);
      
   TRISBbits.TRISB2 = 0;
   TRISBbits.TRISB3 = 1;
   
   RXB0CONbits.RXFUL = 0;  //clear rx and tx buffers
   RXB1CONbits.RXFUL = 0;
   TXB0CONbits.TXREQ = 0;
   TXB1CONbits.TXREQ = 0;
   TXB2CONbits.TXREQ = 0;

   PIE3 = 0;            //CAN interrupts disable
   PIR3 = 0;            //CAN interrupts flags clear
   IPR3 = 0;            //CAN interrupts low priority set
   CIOCON = 0x20;       //can I/O control register

   RXB0CON = 0x24;      //recive valid messages with standard identifier, if overflow, move to buffer1
   RXB1CON = 0x20;      //recive valid messages with standard identifier

   //set filters
   #ifdef USE_HARDWARE_FILTERING
      RXM0SIDH = 0xFF; RXM0SIDL = 0xFF;
      RXM1SIDH = 0xFF; RXM1SIDL = 0xFF;

      RXF0SIDH = BYTE1(CO_RXCAN_ID[0]); RXF0SIDL = BYTE0(CO_RXCAN_ID[0]);
      RXF1SIDH = BYTE1(CO_RXCAN_ID[1]); RXF1SIDL = BYTE0(CO_RXCAN_ID[1]);

      RXF2SIDH = BYTE1(CO_RXCAN_ID[2]); RXF2SIDL = BYTE0(CO_RXCAN_ID[2]);
      RXF3SIDH = 0; RXF3SIDL = 0;
      RXF4SIDH = 0; RXF4SIDL = 0;
      RXF5SIDH = 0; RXF5SIDL = 0;
      #if((3+CO_NO_RPDO+CO_NO_CONS_HEARTBEAT) > 3)
         RXF3SIDH = BYTE1(CO_RXCAN_ID[3]); RXF3SIDL = BYTE0(CO_RXCAN_ID[3]);
      #endif
      #if((3+CO_NO_RPDO+CO_NO_CONS_HEARTBEAT) > 4)
         RXF4SIDH = BYTE1(CO_RXCAN_ID[4]); RXF4SIDL = BYTE0(CO_RXCAN_ID[4]);
      #endif
      #if((3+CO_NO_RPDO+CO_NO_CONS_HEARTBEAT) > 5)
         RXF5SIDH = BYTE1(CO_RXCAN_ID[5]); RXF5SIDL = BYTE0(CO_RXCAN_ID[5]);
      #endif

   #else
      RXM0SIDH = 0; RXM0SIDL = 0;
      RXF0SIDH = 0; RXF0SIDL = 0;
      RXF1SIDH = 0; RXF1SIDL = 0;
   
      RXM1SIDH = 0; RXM1SIDL = 0;
      RXF2SIDH = 0; RXF2SIDL = 0;
      RXF3SIDH = 0; RXF3SIDL = 0;
      RXF4SIDH = 0; RXF4SIDL = 0;
      RXF5SIDH = 0; RXF5SIDL = 0;
   #endif   
   
   CANCON = 0x00;       //request normal mode
   while((CANSTAT & 0xE0) != 0x00); //wait until normal mode is set 

   PIE3 = 0x23;         //enable error and recive interrupts
   PIR3 = 0x1C;         //set transmit buffer interrupt flags
   IPR3 = 0x03;         //recive buffers are high priority
  
}

/*******************************************************************************
   Send CAN messages (SDO, HeartBeat, User)
      INPUT:
         BuffNo   - Number of buffer to be sent
      OUTPUT:
         0 = success
         1 = buffer full
   
*******************************************************************************/
char CO_CanSendOther(unsigned char BuffNo){
   unsigned char flag = 1 << BuffNo;
   
   if(CO_TXCAN_OtherSendReq & flag){
      ErrorReport(ERROR_CanSendOther_Overflow, BuffNo);
      return 1;
   }
   CO_TXCAN_OtherSendReq |= flag;
   PIE3bits.TXB0IE = 1; //inform ISR
   
   return 0;
}

/*******************************************************************************
   SEND CAN MESSAGE TO CAN REGISTERS
      Executed only from Low ISR!
      INPUT:
         CanMessage  - CAN message to be sent
         buffer      - 0=TXB0 (reserved for SDO, Heartbeat, avilable for User), 
                       1=TXB1 (reserved for PDO), 
                       2=TXB2 (reserved for Sync and Emergency)
         priority    - 0=lowest ... 3=highest
      OUTPUT:
         0 = success
         1 = can buffer full
         2 = wrong parameters
   
*******************************************************************************/
char CO_CanSend(CO_CanMessage CanMessage, char buffer, char priority){
   switch(buffer){
    case 0: //set transmit buffer 0 to access memory
      if(PIR3bits.TXB0IF == 0) return 1;  //if buffer is not free
      PIR3bits.TXB0IF = 0;
      CANCONbits.WIN2 = 1; CANCONbits.WIN1 = 0; CANCONbits.WIN0 = 0;//set TXB0 to access memory
      break;
    case 1: //set transmit buffer 1 to access memory
      if(PIR3bits.TXB1IF == 0) return 1;  //if buffer is not free
      PIR3bits.TXB1IF = 0;
      CANCONbits.WIN2 = 0; CANCONbits.WIN1 = 1; CANCONbits.WIN0 = 1;//set TXB1 to access memory
      break;
    case 2: //set transmit buffer 2 to access memory
      if(PIR3bits.TXB2IF == 0) return 1;  //if buffer is not free
      PIR3bits.TXB2IF = 0;
      CANCONbits.WIN2 = 0; CANCONbits.WIN1 = 1; CANCONbits.WIN0 = 0;//set TXB2 to access memory
      break;
    default:
      return 2;
   }

   if(CanMessage.NoOfBytes > 8) CanMessage.NoOfBytes  = 8;

   //put address in buffers, set standard identifier
   RXB0SIDL = BYTE0(CanMessage.COB_ID_LSH5);
   RXB0SIDH = BYTE1(CanMessage.COB_ID_LSH5);
   //set data length, no remote request
   RXB0DLC = CanMessage.NoOfBytes;
   //copy data
   if(CanMessage.NoOfBytes)
      memcpy((void*)&RXB0D0, (void*)CanMessage.pData, CanMessage.NoOfBytes);
   //control register, transmit request, set priority
   RXB0CON = 0x08 | (priority & 0x3);

   return 0;
}

/*******************************************************************************
   ERROR HANDLING
      For description see Errors.h
*******************************************************************************/
//function is used from both: ISR() and main(). Interrupts must NOT be disabled.
void ErrorReport(unsigned char ErrorBit, unsigned char Code){
   char index, bitmask;
   unsigned char TempOld, TempNew;
   
   index = ErrorBit >> 3;
   bitmask = 1 << (ErrorBit & 0x7);
   //if ErrorBit value not supported, set ERROR_ErrorReport_ParametersNotSupp   
   if(index > (ODE_ErrorStatusBitsNoOfEntries-1)){  
      index = 0; bitmask = 2;
   }
   TempOld = ODE_ErrorStatusBits[index];
      TempNew = TempOld | bitmask;  //set error bit
      
   if(TempNew != TempOld){
      if(!(index == 0 && bitmask == 1))   //if NO_ERROR just send emergency
         ODE_ErrorStatusBits[index] = TempNew;
      ErrorControl.CheckErrors = 1;
      if(!ErrorControl.EmergencyToSend){  //free
         ErrorControl.EmergencyErrorCode = ErrorCodesTable[ErrorBit];
         ErrorControl.CodeVal = Code;
         ErrorControl.EmergencyToSend = 1;
      }
   }
}

void ErrorReset(unsigned char ErrorBit, unsigned char Code){
   char index = ErrorBit >> 3;
   char bitmask = 1 << (ErrorBit & 0x7);
   unsigned char TempOld, TempNew;
   
   if(index > (ODE_ErrorStatusBitsNoOfEntries-1)){
      ErrorReport(ERROR_ErrorReport_ParametersNotSupp, ErrorBit);
      return;
   }
   TempOld = ODE_ErrorStatusBits[index];
   TempNew = TempOld & (~bitmask);  //reset error bit
      
   if(TempNew != TempOld){
      ODE_ErrorStatusBits[index] = TempNew;
      ErrorControl.CheckErrors = 1;
      if(!ErrorControl.EmergencyToSend){  //free
         ErrorControl.EmergencyErrorCode = 0x0000;
         ErrorControl.CodeVal = Code;
         ErrorControl.EmergencyToSend = 1;
      }
   }
}

#pragma interrupt CO_IsrHigh save = CANCON
void CO_IsrHigh(void){
   PCB_BANDWIDTH_IsrHigh(1);
   CO_IsrHighIndex = 0;
   if(PIR3bits.RXB0IF){ //RXB0 occured
      PIR3bits.RXB0IF = 0;
      CANCONbits.WIN2 = 1; CANCONbits.WIN1 = 1; CANCONbits.WIN0 = 0;//set RXB0 to access memory
   }
   else{ //RXB1 occured
      PIR3bits.RXB1IF = 0;
      CANCONbits.WIN2 = 1; CANCONbits.WIN1 = 0; CANCONbits.WIN0 = 1;//set RXB1 to access memory
   }