File content as of revision 0:aa420961d99b:

#include"mbed.h"
#include"string.h"
#include "CAN.h"
#include"can.h"
#define node_ID 4
CAN can1(p30, p29); // rd, td Monitor
DigitalOut myled1(LED1);
DigitalOut myled2(LED2);
DigitalOut myled3(LED3);
DigitalOut myled4(LED4);

bool newData = false;


int counter = 0;
int counter1 = 0;
float vspeed = 0.0;
float vattitude = 0.0;



void can::CAN2_wrFilter (uint32_t id)  {
    static int CAN_std_cnt = 0;
    uint32_t buf0, buf1;
    int cnt1, cnt2, bound1;
 
    /* Acceptance Filter Memory full */
    if (((CAN_std_cnt + 1) >> 1) >= 512)
        return;                                       /* error: objects full */
 
    /* Setup Acceptance Filter Configuration
      Acceptance Filter Mode Register = Off  */
    LPC_CANAF->AFMR = 0x00000001;
 
    id |= 1 << 13;                        /* Add controller number(2) */
    id &= 0x0000F7FF;                            /* Mask out 16-bits of ID */
 
    if (CAN_std_cnt == 0)  {                     /* For entering first  ID */
        LPC_CANAF_RAM->mask[0] = 0x0000FFFF | (id << 16);
    }  else if (CAN_std_cnt == 1)  {             /* For entering second ID */
        if ((LPC_CANAF_RAM->mask[0] >> 16) > id)
            LPC_CANAF_RAM->mask[0] = (LPC_CANAF_RAM->mask[0] >> 16) | (id << 16);
        else
            LPC_CANAF_RAM->mask[0] = (LPC_CANAF_RAM->mask[0] & 0xFFFF0000) | id;
    }  else  {
        /* Find where to insert new ID */
        cnt1 = 0;
        cnt2 = CAN_std_cnt;
        bound1 = (CAN_std_cnt - 1) >> 1;
        while (cnt1 <= bound1)  {                  /* Loop through standard existing IDs */
            if ((LPC_CANAF_RAM->mask[cnt1] >> 16) > id)  {
                cnt2 = cnt1 * 2;
                break;
            }
            if ((LPC_CANAF_RAM->mask[cnt1] & 0x0000FFFF) > id)  {
                cnt2 = cnt1 * 2 + 1;
                break;
            }
            cnt1++;                                  /* cnt1 = U32 where to insert new ID */
        }                                          /* cnt2 = U16 where to insert new ID */
 
        if (cnt1 > bound1)  {                      /* Adding ID as last entry */
            if ((CAN_std_cnt & 0x0001) == 0)         /* Even number of IDs exists */
                LPC_CANAF_RAM->mask[cnt1]  = 0x0000FFFF | (id << 16);
            else                                     /* Odd  number of IDs exists */
                LPC_CANAF_RAM->mask[cnt1]  = (LPC_CANAF_RAM->mask[cnt1] & 0xFFFF0000) | id;
        }  else  {
            buf0 = LPC_CANAF_RAM->mask[cnt1];        /* Remember current entry */
            if ((cnt2 & 0x0001) == 0)                /* Insert new mask to even address */
                buf1 = (id << 16) | (buf0 >> 16);
            else                                     /* Insert new mask to odd  address */
                buf1 = (buf0 & 0xFFFF0000) | id;
 
            LPC_CANAF_RAM->mask[cnt1] = buf1;        /* Insert mask */
 
            bound1 = CAN_std_cnt >> 1;
            /* Move all remaining standard mask entries one place up */
            while (cnt1 < bound1)  {
                cnt1++;
                buf1  = LPC_CANAF_RAM->mask[cnt1];
                LPC_CANAF_RAM->mask[cnt1] = (buf1 >> 16) | (buf0 << 16);
                buf0  = buf1;
            }
 
            if ((CAN_std_cnt & 0x0001) == 0)         /* Even number of IDs exists */
                LPC_CANAF_RAM->mask[cnt1] = (LPC_CANAF_RAM->mask[cnt1] & 0xFFFF0000) | (0x0000FFFF);
        }
    }
    CAN_std_cnt++;
 
    /* Calculate std ID start address (buf0) and ext ID start address <- none (buf1) */
    buf0 = ((CAN_std_cnt + 1) >> 1) << 2;
    buf1 = buf0;
 
    /* Setup acceptance filter pointers */
    LPC_CANAF->SFF_sa     = 0;
    LPC_CANAF->SFF_GRP_sa = buf0;
    LPC_CANAF->EFF_sa     = buf0;
    LPC_CANAF->EFF_GRP_sa = buf1;
    LPC_CANAF->ENDofTable = buf1;
 
    LPC_CANAF->AFMR = 0xFFFF;                  /* Use acceptance filter */
} // CAN2_wrFilter
 

void can::sendtocanvattitude(){
    char datas[4];
    char data1[4];
    char canmsg[8];
    if(newData==true){
   
    if (counter==255){
        counter=0;
    }
    else{
    counter=++counter;
    datas[0] = (char)(0x00); 
    datas[1] = (char)(0x02); 
    datas[2] = (char)(0x00);
    datas[3] = (char)(counter);
    memcpy(data1,&vattitude,4);
    memcpy(canmsg, datas, 4);
    memcpy(canmsg+4, data1, 8);
    if(can1.write(CANMessage(311,canmsg,8))) {
            printf("Node_ID 311 sent \n");
    }
    myled1 = !myled1;
    newData = false;
    }
    }
}

void can::sendtocanvspeed(){
    char data2[4];
    char datass[4];
    char canmsg1[8];
    if(newData==true){
     if (counter1==255){
        counter1=0;
    }else
    {
    counter1=++counter1;
    datass[0] = (char)(0x00); 
    datass[1] = (char)(0x02); 
    datass[2] = (char)(0x00);
    datass[3] = (char)(counter1);
    memcpy(data2,&vspeed,4);
    memcpy(canmsg1, datass, 4);
    memcpy(canmsg1+4, data2, 8);
    
    
    if(can1.write(CANMessage(1124,canmsg1,8))) {
            printf("Node_ID 1124 sent \n");
        }
    myled2 = !myled2;
    newData = false;
    }
    }
}





void can::tick(){
    ticker.attach(callback(this, &can::sendtocanvspeed), 1.0 );
}



////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////
////////////////NODE SERVICE PROTOCOL///////////////////////
////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////
//////// identifiction service:
void nodespresponse::ids(){
    char data[8];
    data[0] = (char)(node_ID);//node_id
    data[1] = (char)(0x10);//data type:uchar4
    data[2] = (char)(0x00);//service code 0 ids
    data[3] = (char)(0x00);//message code as in request 
    data[4] = (char)(0x00);//hardware revision
    data[5] = (char)(0x00);//software revision
    data[6] = (char)(0x00);//identifier distribution (0=CANaerospace distribution)
    data[7] = (char)(0x00);//header type  (0 = CANaerospace standard header)
    if(can1.write(CANMessage(129,data,8))) {
            printf("ids responce from %d is sent",node_ID);
        }  
}
/* nss dont request a response
void nodespresponse::nss(unsigned long time){
    char data[8];
    char data1[4];
    char data2[4]
    char canmsg[8];
    data1[0] = (char)(node_ID);//node_id
    data1[1] = (char)(0x04);//data type:no data
    data1[2] = (char)(0x01);//service code 1
    data1[3] = (char)(0x00);//message code as in request 
    memcpy(data2,&time,4);
    memcpy(canmsg, data1, 4);
    memcpy(canmsg+4, data2, 8);
    can1.write(CANMessage(1124,canmsg,8));
}
*/


/////////////////////////data download service
//////first response a start download request message
void nodespresponse::dds1(){
    char data[8];
    data[0] = (char)(node_ID);//node_id
    data[1] = (char)(0x03);//data type:long
    data[2] = (char)(0x02);//service code 2
    data[3] = (char)(0x00);//as in request
    data[4] = (char)(0x00);/////-2=invalid//-1=abort//0=xoff//1=xon
    data[5] = (char)(0x00);
    data[6] = (char)(0x00);
    data[7] = (char)(0x00);
}
////////second response the message
void nodespresponse::dds2(){
    char data[8];
    data[0] = (char)(node_ID);//node_id
    data[1] = (char)(0x16);//data type:chksum
    data[2] = (char)(0x02);//service code 2
    data[3] = (char)(0x00);//last number
    data[4] = (char)(0x00);//checksum
    data[5] = (char)(0x00);
    data[6] = (char)(0x00);
    data[7] = (char)(0x00);
}
/////////////////////////data upload service
//////first response a start download request message
void nodespresponse::dus1(){
    char data[8];
    data[0] = (char)(node_ID);//node_id
    data[1] = (char)(0x15);//data type:MEMID
    data[2] = (char)(0x03);//service code 3
    data[3] = (char)(0x00);
    data[4] = (char)(0x00);
    data[5] = (char)(0x00);
    data[6] = (char)(0x00);
    data[7] = (char)(0x00);
}
////////second response the message
void nodespresponse::dus2(){
    char data[8];
    data[0] = (char)(node_ID);//node_id
    data[1] = (char)(0x15);//data type:MEMID
    data[2] = (char)(0x03);//service code 3
    data[3] = (char)(0x00); 
    data[4] = (char)(0x00);
    data[5] = (char)(0x00);
    data[6] = (char)(0x00);
    data[7] = (char)(0x00);
}

////////////simulation control service

void nodespresponse::scs(){
    char data[8];
    data[0] = (char)(node_ID);//node_id
    data[1] = (char)(0x00);//data type:??????????? user defined
    data[2] = (char)(0x04);//service code 4
    data[3] = (char)(0x00);//message code:???????????? 
    data[4] = (char)(0x00);/////user defined
    data[5] = (char)(0x00);
    data[6] = (char)(0x00);
    data[7] = (char)(0x00);
}

//////Transmission Interval Service

void nodespresponse::tis(){
    char data[8];
    data[0] = (char)(node_ID);//node_id
    data[1] = (char)(0x00);//data type:no data
    data[2] = (char)(0x05);//service code 
    data[3] = (char)(0x00);//message code:0=ok//-6 =CAN identifier or transmission rate out of range
    data[4] = (char)(0x00);/////na
    data[5] = (char)(0x00);/////na
    data[6] = (char)(0x00);////na
    data[7] = (char)(0x00);////na
}
//////FLASH Programming Service

void nodespresponse::fps(){
    char data[8];
    data[0] = (char)(node_ID);//node_id
    data[1] = (char)(0x00);//data type:nodata
    data[2] = (char)(0x06);//service code 6
    data[3] = (char)(0x00);//message code:0 = OK ///-3 = invalid security code
    data[4] = (char)(0x00);/////na
    data[5] = (char)(0x00);
    data[6] = (char)(0x00);
    data[7] = (char)(0x00);
}

/* no response
//////State Transmission Service

void nodespresponse::sts(){
    char data[8];
    data[0] = (char)(node_ID);//node_id
    data[1] = (char)(0x00);//data type:nodata
    data[2] = (char)(0x07);//service code 7
    data[3] = (char)(0x00);//message code:0
    data[4] = (char)(0x00);/////na
    data[5] = (char)(0x00);
    data[6] = (char)(0x00);
    data[7] = (char)(0x00);
}
*/

//////Filter setting services

void nodespresponse::fss(){
    char data[8];
    data[0] = (char)(node_ID);//node_id
    data[1] = (char)(0x00);//data type:no data
    data[2] = (char)(0x08);//service code 8
    data[3] = (char)(0x00);//message code:0 = OK -6 = limit frequency or filter number out of rang
    data[4] = (char)(0x00);/////na
    data[5] = (char)(0x00);
    data[6] = (char)(0x00);
    data[7] = (char)(0x00);
}



//////Test Control Service 

void nodespresponse::tcs(){
    char data[8];
    data[0] = (char)(node_ID);//node_id
    data[1] = (char)(0x00);//data type:user defined
    data[2] = (char)(0x09);//service code 9
    data[3] = (char)(0x00);//message code:user defined
    data[4] = (char)(0x00);/////user defined
    data[5] = (char)(0x00);
    data[6] = (char)(0x00);
    data[7] = (char)(0x00);
}



//////Baudrate Setting Service

void nodespresponse::bss(){
    char data[8];
    data[0] = (char)(node_ID);//node_id
    data[1] = (char)(0x06);//data type:short
    data[2] = (char)(0xA);//service code 10
    data[3] = (char)(0x00);//message code:-1 = baudrate code unknown
    data[4] = (char)(0x00);/////na
    data[5] = (char)(0x00);/////na
    data[6] = (char)(0x00);///na
    data[7] = (char)(0x00);////na
}


//////Node-ID Setting Service

void nodespresponse::nis(){
    char data[8];
    data[0] = (char)(node_ID);//node_id
    data[1] = (char)(0x00);//data type:no data
    data[2] = (char)(0xB);//service code 11
    data[3] = (char)(0x00);//message code:0 = OK -6 = node-ID out of range
    data[4] = (char)(0x00);/////na
    data[5] = (char)(0x00);
    data[6] = (char)(0x00);
    data[7] = (char)(0x00);
}

////// Module Information Service 

void nodespresponse::mis(){
    char data[8];
    data[0] = (char)(node_ID);//node_id
    data[1] = (char)(0x00);//data type:user defined
    data[2] = (char)(0xC);//service code 12
    data[3] = (char)(0x00);//message code:user defined
    data[4] = (char)(0x00);/////user defined
    data[5] = (char)(0x00);
    data[6] = (char)(0x00);
    data[7] = (char)(0x00);
}
//////  Module Configuration Service

void nodespresponse::mcs(){
    char data[8];
    data[0] = (char)(node_ID);//node_id
    data[1] = (char)(0x00);//data type:user defined
    data[2] = (char)(0xD);//service code 13
    data[3] = (char)(0x00);//message code:user defined
    data[4] = (char)(0x00);/////user defined
    data[5] = (char)(0x00);
    data[6] = (char)(0x00);
    data[7] = (char)(0x00);
}
////// CAN Identifier Setting Service

void nodespresponse::css(){
    char data[8];
    data[0] = (char)(node_ID);//node_id
    data[1] = (char)(0x00);//data type:no data
    data[2] = (char)(0xE);//service code 14
    data[3] = (char)(0x00);//message code:0 = OK -6 = message number or CAN identifier out of range
    data[4] = (char)(0x00);/////na
    data[5] = (char)(0x00);/////na
    data[6] = (char)(0x00);////na
    data[7] = (char)(0x00);///na
}

//////CANaerospace Identifier Distribution Setting Service

void nodespresponse::dss(){
    char data[8];
    data[0] = (char)(node_ID);//node_id
    data[1] = (char)(0x00);//data type:no data
    data[2] = (char)(0xF);//service code 15
    data[3] = (char)(0x00);//message code:0= ok //-6= distribution id out of range
    data[4] = (char)(0x00);/////na
    data[5] = (char)(0x00);
    data[6] = (char)(0x00);
    data[7] = (char)(0x00);
}