///////////////////////////////////////////////////////////////
//            I-87120 firmware for user design demo          //
//                    CANopen I/O control                    //
//   Demo devices: I-87120 + CANopen DI/DO/AI/AO devices     //
//                                                           // 
///////////////////////////////////////////////////////////////
#include "..\..\c87s110.h"
#include <stdio.h>
#include <string.h>
#include <stdlib.h>


unsigned long tx,rx,irq;
                                          
void UserIrqFunc(char INTT)               
{
  //  If CAN bus interrupt has occur, the irq++ will be excuted
  if (INTT) irq++;  
}

void UserInitFunc(void)
{
  //  The function will be excuted first after I-87120 is power on
  tx=0;
  rx=0;
  irq=0;
}

void UserDefCmdFunc(unsigned int CmdLength, char * UserCmd)
{
  // User define command, the UserCmd[0] must be '_' 
  unsigned char data[8];
  int Ret;

  if (CmdLength<2){
    ToCom1Str(">CmdErr\r");
    return;
  }
  switch (UserCmd[1]){
    case 'D': //Send CANopen DO/DI message
     {
       switch(UserCmd[2]){
         case 'O':// _DOiiiccdd
                  // _DO: command , iii: cob id , cc: channel(start from 1) , dd: output data
           {
            char TempBuf[10],*endptr;
            unsigned char ch;
            unsigned long cobid;
          
            memcpy(TempBuf,&UserCmd[3],3);
            TempBuf[3]=0;
            cobid = (unsigned long)(strtol(TempBuf,&endptr,16)&0x7ff);
            
            memcpy(TempBuf,&UserCmd[6],2);
            TempBuf[2]=0;
            ch = (unsigned char)(strtol(TempBuf,&endptr,16)&0xff);
            
            memset(data,0,8);

            memcpy(TempBuf,&UserCmd[8],2);
            TempBuf[2]=0;
            data[4] = (unsigned char)(strtol(TempBuf,&endptr,16)&0xff);
            
            data[0] = 0x2f;
            data[1] = 0x00;
            data[2] = 0x62;
            data[3] = ch;
            
            Ret=SendCANMsg(0,cobid,0,5,data);
            if (Ret) ToCom1Str(">SendFaile\r");
            else{ 
              tx++;
            }
           }
            break;
         case 'I':// _DIiiicc
                  // _DI: command , iii: cob id , cc: channel(start from 1)
           {
            char TempBuf[10],*endptr;
            unsigned char ch;
            unsigned long cobid;
          
            memcpy(TempBuf,&UserCmd[3],3);
            TempBuf[3]=0;
            cobid = (unsigned long)(strtol(TempBuf,&endptr,16)&0x7ff);
            
            memcpy(TempBuf,&UserCmd[6],2);
            TempBuf[2]=0;
            ch = (unsigned char)(strtol(TempBuf,&endptr,16)&0xff);
            
            memset(data,0,8);
            
            data[0] = 0x40;
            data[1] = 0x00;
            data[2] = 0x60;
            data[3] = ch;
            
            Ret=SendCANMsg(0,cobid,0,4,data);
            if (Ret) ToCom1Str(">SendFaile\r");
            else{ 
              tx++;
            }
           }
            break;
       }
       break;
     }
   case 'A': //Send CANopen AO/AI message
     {
       switch(UserCmd[2]){
         case 'O':// _AOiiiccdddd
                  // _AO: command , iii: cob id , cc: channel(start from 1) , dddd: output data
           {
            char TempBuf[10],*endptr;
            unsigned char ch;
            unsigned long cobid;
          
            memcpy(TempBuf,&UserCmd[3],3);
            TempBuf[3]=0;
            cobid = (unsigned long)(strtol(TempBuf,&endptr,16)&0x7ff);
            
            memcpy(TempBuf,&UserCmd[6],2);
            TempBuf[2]=0;
            ch = (unsigned char)(strtol(TempBuf,&endptr,16)&0xff);
            
            memset(data,0,8);

            memcpy(TempBuf,&UserCmd[8],2);
            TempBuf[2]=0;
            data[5] = (unsigned char)(strtol(TempBuf,&endptr,16)&0xff);
            
            memcpy(TempBuf,&UserCmd[10],2);
            TempBuf[2]=0;
            data[4] = (unsigned char)(strtol(TempBuf,&endptr,16)&0xff);
            
            data[0] = 0x2b;
            data[1] = 0x11;
            data[2] = 0x64;
            data[3] = ch;
            
            Ret=SendCANMsg(0,cobid,0,6,data);
            if (Ret) ToCom1Str(">SendFaile\r");
            else{ 
              tx++;
            }
           }
            break;
         case 'I':// _AIiiicc
                  // _AI: command , iii: cob id , cc: channel(start from 1)
           {
            char TempBuf[10],*endptr;
            unsigned char ch;
            unsigned long cobid;
          
            memcpy(TempBuf,&UserCmd[3],3);
            TempBuf[3]=0;
            cobid = (unsigned long)(strtol(TempBuf,&endptr,16)&0x7ff);
            
            memcpy(TempBuf,&UserCmd[6],2);
            TempBuf[2]=0;
            ch = (unsigned char)(strtol(TempBuf,&endptr,16)&0xff);
            
            memset(data,0,8);
            
            data[0] = 0x40;
            data[1] = 0x01;
            data[2] = 0x64;
            data[3] = ch;
            
            Ret=SendCANMsg(0,cobid,0,4,data);
            if (Ret) ToCom1Str(">SendFaile\r");
            else{ 
              tx++;
            }
           }
            break;
       }
       break;
     }
   case 'T': 
     switch(UserCmd[2]){
      case 's':// Total send message count
        {
         char strtemp[50];
        
         sprintf(strtemp,"Total send CAN message:%ld\r",tx);
         ToCom1Str(strtemp);
        }
        break;
      case 'r':// Total receive message count
        {
         char strtemp[50];
         
         sprintf(strtemp,"Total receive CAN message:%ld\r",rx);
         ToCom1Str(strtemp);
        }
        break;
     }
     break;
   default:
    ToCom1Str(">CmdNoSupport\r");
  }
}

void UserDefBinaryFunc(unsigned int CmdLength, char * UserCmd)
{
  // Process user-defined command for binary code, the UserCmd[0] must be '|' 
  if (CmdLength);
  switch (UserCmd[1]){
  }
}

void UserLoopFunc(void)   
{
  // The function will be excuted in while loop after the I-87120 has be configured
  int Ret,i;
  unsigned char Mode,RTR,DataLen,Data[8];
  unsigned long ID;
  char strtemp[100],cattemp[20];
  unsigned char dlen;
  
  Ret=GetCANMsg(&Mode,&ID,&RTR,&DataLen,Data);  // Loop for receive CAN message
  if(!Ret){
    rx++;
    dlen = DataLen;
    if(RTR){ //  RTR
      if(Mode){  //  29 bits id
        sprintf(strtemp,"RTR: id:%08X , ",ID);
      }
      else{             //  11 bits id
        sprintf(strtemp,"RTR: id:%03X , ",ID);
      }
      sprintf(cattemp,"dlen:%d",dlen);
      strcat(strtemp,cattemp);
      
      if(dlen){
        sprintf(cattemp," , data: ");
        strcat(strtemp,cattemp);
        for(i=0;i<dlen;i++){
          sprintf(cattemp,"%02X,",Data[i]);
          strcat(strtemp,cattemp);
        }
        strcat(strtemp,"\r\n");
      }
      else{
        strcat(strtemp,"\r\n");
      }
      ToCom1Str(strtemp);
    }
    else{ //  Not RTR
      if(Mode){  //  29 bits id
        sprintf(strtemp,"Msg: id:%08X , ",ID);
      }
      else{             //  11 bits id
        sprintf(strtemp,"Msg: id:%03X , ",ID);
      }
      sprintf(cattemp,"dlen:%d",dlen);
      strcat(strtemp,cattemp);
      
      if(dlen){
        sprintf(cattemp," , data: ");
        strcat(strtemp,cattemp);
        for(i=0;i<dlen;i++){
          sprintf(cattemp,"%02X,",Data[i]);
          strcat(strtemp,cattemp);
        }
        strcat(strtemp,"\r\n");
      }
      else{
        strcat(strtemp,"\r\n");
      }
      //Print("[%d][%s]\r\n",dlen,strtemp);
      ToCom1Str(strtemp);
    }
  }
}