/***************************應用層CANOpen處理written by Meng Zhao   BJUT****************************/

#include "can.h"

#include "canpc.h"
#include "REG591.h"
#include "string.h"

CAN_MSG data rec_mes;
CAN_MSG data TX_SDO;
CANOpenConfig data CANOpenpara;
unsigned char data state;
unsigned int data RecSDO_ID;
unsigned int data RecPDO_ID;
extern unsigned char rec_status;
extern unsigned char send_status;
BYTE RX_message[13];


unsigned char const code SDOTable[]={
   // [1000h,00]: Device Type
  SDOREPLY(0x1000, 0x00, 4, OD_DEVICE_TYPE),

#ifdef OD_SERIAL
  // [1018h,00]: Identity Object, Number of Entries = 4
  SDOREPLY(0x1018, 0x00, 1, 0x00000004L),
#else
  // [1018h,00]: Identity Object, Number of Entries = 3
  SDOREPLY(0x1018, 0x00, 1, 0x00000003L),
#endif

  // [1018h,01]: Identity Object, Vendor ID
  SDOREPLY(0x1018, 0x01, 4, OD_VENDOR_ID),

  // [1018h,02]: Identity Object, Product Code
  SDOREPLY(0x1018, 0x02, 4, OD_PRODUCT_CODE),

  // [1018h,03]: Identity Object, Revision
  SDOREPLY(0x1018, 0x03, 4, OD_REVISION),

#ifdef OD_SERIAL
  // [1018h,04]: Identity Object, Serial
  SDOREPLY(0x1018, 0x04, 4, OD_SERIAL),
#endif

  // [2018h,00]: MicroCANopen Identity Object, Number of Entries = 3
  SDOREPLY(0x2018, 0x00, 1, 0x00000003L),

  // [2018h,01]: MicroCANopen Identity Object, Vendor ID = 01455341, ESA Inc.
  SDOREPLY(0x2018, 0x01, 4, 0x01455341L),

  // [2018h,02]: MicroCANopen Identity Object, Product Code = "MCOP"
  SDOREPLY4(0x2018, 0x02, 4, 'P', 'O', 'C', 'M'),

  // [2018h,03]: MicroCANopen Identity Object, Revision = 1.20
  SDOREPLY(0x2018, 0x03, 4, 0x00010020L),

#ifdef PDO_IN_OD
  // NOTE: These entries must be added manually. The parameters must match
  // the parameters used to call the functions MCO_InitRPDO and MCO_InitTPDO.

  // These entries are necessary to be fully CANopen compliant.
  // Suppported in commercial version of MicroCANopen available from
  // www.CANopenStore.com

  // Warning: This version is not fully CANopen compliant - PDO_IN_OD must not be defined
  #error Warning: This version of MicroCANopen has a limited Object Dictionary! Un-define PDO_IN_OD to confirm!
#endif // PDO_IN_OD

  // End-of-table marker
  SDOREPLY(0xFFFF, 0xFF, 0xFF, 0xFFFFFFFFL)
};
void main()
{
  Can_init();      
  
  reset_communication();
  while(1)
  {
  process();
  }
}

void CANOpen_init(unsigned char NodeID)

{
  RecPDO_ID=0X200+NodeID;
  RecSDO_ID=0X600+NodeID;
  TX_SDO.ID=0X580+NodeID;
  TX_SDO.LEN=0X08;
   CANOpenpara.NodeID=NodeID;
//設置3個濾波器分別以NMT,接收SDO,接收PDO報文的ID作為驗證ID,這樣如果接收到的報文ID與三個報文驗證ID之一相符合，則報文寫入接收BUF.//
  SetCanfilter(0,0,0,0X00,0X0F,0XFF,0XFF);//set canfilter for receive NMT message.
  SetCanfilter(1,RecSDO_ID,0,0X00,0X0F,0XFF,0XFF);//set canfilter for receive SDO message.
  SetCanfilter(2,RecPDO_ID,0,0X00,0X0F,0XFF,0XFF);//set canfilter for receive PDO message.

}
unsigned char CANPC_Search(unsigned int Cindex,unsigned char Csubindex)
{
  unsigned char i;
  unsigned char index_high,hi;
  unsigned char index_low,lo;
  unsigned char const *P;
  unsigned char const *r;
  i=0;
 index_high=(unsigned char)(Cindex>>8);
index_low=(unsigned char)Cindex;

  r=&(SDOTable[0]);
  while(i<255)
    {
	 P=r;
	 r+=8;
	 P++;
	 lo=*P;
	 P++;
	 hi=*P;
       if((index_low==0XFF)&&(index_high==0XFF))
	     return 255;
	   if((index_low==lo)&&(index_high==hi))
	     {
		  P++;
		  if(*P==Csubindex)
		    return i;
		  }
	   i++;
	   }
      return 255;
}





unsigned char Handle_SDO(unsigned char *Cdata)
 {
  unsigned char cmd;
  unsigned int index;
  unsigned char subindex;
  unsigned char num;
  cmd=Cdata[0]&0XE0;
  index=Cdata[2];
  index=(index<<8)+Cdata[1];
  subindex=Cdata[3];
  if((cmd==0X20)||(cmd==0X40))
   {
     num=CANPC_Search(index,subindex);
       if(num<255)
	     {
		   if(cmd==0X40)
		     {
			   memcpy(&TX_SDO.BUF[0],&(SDOTable[num*8]),8);
			   Trans_message(&TX_SDO);
			   return 1;
			  }
			return 0;
		  }
     }
   }


void process()
{
Rec_message(&rec_mes);
if(rec_status)//check if there is a rec_mes
  {
    rec_status=0;
	if(rec_mes.ID==0)//NMT message?
	  {
	    switch(rec_mes.BUF[0])
		  {
		    case 1:     //start(operational )
			 state=1;
			 break;
            case 2:    //stop
			 state=2;
			 break;
			case 128:  //pre-operational
			 state=128;
			 break;
			case 129:  //reset
			 state=129;
			 reset_communication();
			 break;
			case 130:  //reset commuciation
			 state=130;
			 reset_communication();
			 break;
		  }
      }

    if(state!=2)
      if(rec_mes.ID== RecSDO_ID)
        Handle_SDO(rec_mes.BUF);
	}
 }