Diff: can/can.cpp

Revision:

0:aa420961d99b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/can/can.cpp	Tue Jun 15 10:29:50 2021 +0000
@@ -0,0 +1,432 @@
+#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);
+}