can.cpp

00001 #include"mbed.h"
00002 #include"string.h"
00003 #include "CAN.h"
00004 #include"can.h"
00005 #define node_ID 4
00006 CAN can1(p30, p29); // rd, td Monitor
00007 DigitalOut myled1(LED1);
00008 DigitalOut myled2(LED2);
00009 DigitalOut myled3(LED3);
00010 DigitalOut myled4(LED4);
00011 
00012 bool newData = false;
00013 
00014 
00015 int counter = 0;
00016 int counter1 = 0;
00017 float vspeed = 0.0;
00018 float vattitude = 0.0;
00019 
00020 
00021 
00022 void can::CAN2_wrFilter (uint32_t id)  {
00023     static int CAN_std_cnt = 0;
00024     uint32_t buf0, buf1;
00025     int cnt1, cnt2, bound1;
00026  
00027     /* Acceptance Filter Memory full */
00028     if (((CAN_std_cnt + 1) >> 1) >= 512)
00029         return;                                       /* error: objects full */
00030  
00031     /* Setup Acceptance Filter Configuration
00032       Acceptance Filter Mode Register = Off  */
00033     LPC_CANAF->AFMR = 0x00000001;
00034  
00035     id |= 1 << 13;                        /* Add controller number(2) */
00036     id &= 0x0000F7FF;                            /* Mask out 16-bits of ID */
00037  
00038     if (CAN_std_cnt == 0)  {                     /* For entering first  ID */
00039         LPC_CANAF_RAM->mask[0] = 0x0000FFFF | (id << 16);
00040     }  else if (CAN_std_cnt == 1)  {             /* For entering second ID */
00041         if ((LPC_CANAF_RAM->mask[0] >> 16) > id)
00042             LPC_CANAF_RAM->mask[0] = (LPC_CANAF_RAM->mask[0] >> 16) | (id << 16);
00043         else
00044             LPC_CANAF_RAM->mask[0] = (LPC_CANAF_RAM->mask[0] & 0xFFFF0000) | id;
00045     }  else  {
00046         /* Find where to insert new ID */
00047         cnt1 = 0;
00048         cnt2 = CAN_std_cnt;
00049         bound1 = (CAN_std_cnt - 1) >> 1;
00050         while (cnt1 <= bound1)  {                  /* Loop through standard existing IDs */
00051             if ((LPC_CANAF_RAM->mask[cnt1] >> 16) > id)  {
00052                 cnt2 = cnt1 * 2;
00053                 break;
00054             }
00055             if ((LPC_CANAF_RAM->mask[cnt1] & 0x0000FFFF) > id)  {
00056                 cnt2 = cnt1 * 2 + 1;
00057                 break;
00058             }
00059             cnt1++;                                  /* cnt1 = U32 where to insert new ID */
00060         }                                          /* cnt2 = U16 where to insert new ID */
00061  
00062         if (cnt1 > bound1)  {                      /* Adding ID as last entry */
00063             if ((CAN_std_cnt & 0x0001) == 0)         /* Even number of IDs exists */
00064                 LPC_CANAF_RAM->mask[cnt1]  = 0x0000FFFF | (id << 16);
00065             else                                     /* Odd  number of IDs exists */
00066                 LPC_CANAF_RAM->mask[cnt1]  = (LPC_CANAF_RAM->mask[cnt1] & 0xFFFF0000) | id;
00067         }  else  {
00068             buf0 = LPC_CANAF_RAM->mask[cnt1];        /* Remember current entry */
00069             if ((cnt2 & 0x0001) == 0)                /* Insert new mask to even address */
00070                 buf1 = (id << 16) | (buf0 >> 16);
00071             else                                     /* Insert new mask to odd  address */
00072                 buf1 = (buf0 & 0xFFFF0000) | id;
00073  
00074             LPC_CANAF_RAM->mask[cnt1] = buf1;        /* Insert mask */
00075  
00076             bound1 = CAN_std_cnt >> 1;
00077             /* Move all remaining standard mask entries one place up */
00078             while (cnt1 < bound1)  {
00079                 cnt1++;
00080                 buf1  = LPC_CANAF_RAM->mask[cnt1];
00081                 LPC_CANAF_RAM->mask[cnt1] = (buf1 >> 16) | (buf0 << 16);
00082                 buf0  = buf1;
00083             }
00084  
00085             if ((CAN_std_cnt & 0x0001) == 0)         /* Even number of IDs exists */
00086                 LPC_CANAF_RAM->mask[cnt1] = (LPC_CANAF_RAM->mask[cnt1] & 0xFFFF0000) | (0x0000FFFF);
00087         }
00088     }
00089     CAN_std_cnt++;
00090  
00091     /* Calculate std ID start address (buf0) and ext ID start address <- none (buf1) */
00092     buf0 = ((CAN_std_cnt + 1) >> 1) << 2;
00093     buf1 = buf0;
00094  
00095     /* Setup acceptance filter pointers */
00096     LPC_CANAF->SFF_sa     = 0;
00097     LPC_CANAF->SFF_GRP_sa = buf0;
00098     LPC_CANAF->EFF_sa     = buf0;
00099     LPC_CANAF->EFF_GRP_sa = buf1;
00100     LPC_CANAF->ENDofTable = buf1;
00101  
00102     LPC_CANAF->AFMR = 0xFFFF;                  /* Use acceptance filter */
00103 } // CAN2_wrFilter
00104  
00105 
00106 void can::sendtocanvattitude(){
00107     char datas[4];
00108     char data1[4];
00109     char canmsg[8];
00110     if(newData==true){
00111    
00112     if (counter==255){
00113         counter=0;
00114     }
00115     else{
00116     counter=++counter;
00117     datas[0] = (char)(0x00); 
00118     datas[1] = (char)(0x02); 
00119     datas[2] = (char)(0x00);
00120     datas[3] = (char)(counter);
00121     memcpy(data1,&vattitude,4);
00122     memcpy(canmsg, datas, 4);
00123     memcpy(canmsg+4, data1, 8);
00124     if(can1.write(CANMessage(311,canmsg,8))) {
00125             printf("Node_ID 311 sent \n");
00126     }
00127     myled1 = !myled1;
00128     newData = false;
00129     }
00130     }
00131 }
00132 
00133 void can::sendtocanvspeed(){
00134     char data2[4];
00135     char datass[4];
00136     char canmsg1[8];
00137     if(newData==true){
00138      if (counter1==255){
00139         counter1=0;
00140     }else
00141     {
00142     counter1=++counter1;
00143     datass[0] = (char)(0x00); 
00144     datass[1] = (char)(0x02); 
00145     datass[2] = (char)(0x00);
00146     datass[3] = (char)(counter1);
00147     memcpy(data2,&vspeed,4);
00148     memcpy(canmsg1, datass, 4);
00149     memcpy(canmsg1+4, data2, 8);
00150     
00151     
00152     if(can1.write(CANMessage(1124,canmsg1,8))) {
00153             printf("Node_ID 1124 sent \n");
00154         }
00155     myled2 = !myled2;
00156     newData = false;
00157     }
00158     }
00159 }
00160 
00161 
00162 
00163 
00164 
00165 void can::tick(){
00166     ticker.attach(callback(this, &can::sendtocanvspeed), 1.0 );
00167 }
00168 
00169 
00170 
00171 ////////////////////////////////////////////////////////////
00172 ////////////////////////////////////////////////////////////
00173 ///////////////////////////////////////////////////////////
00174 ////////////////NODE SERVICE PROTOCOL///////////////////////
00175 ////////////////////////////////////////////////////////////
00176 ////////////////////////////////////////////////////////////
00177 ///////////////////////////////////////////////////////////
00178 //////// identifiction service:
00179 void nodespresponse::ids(){
00180     char data[8];
00181     data[0] = (char)(node_ID);//node_id
00182     data[1] = (char)(0x10);//data type:uchar4
00183     data[2] = (char)(0x00);//service code 0 ids
00184     data[3] = (char)(0x00);//message code as in request 
00185     data[4] = (char)(0x00);//hardware revision
00186     data[5] = (char)(0x00);//software revision
00187     data[6] = (char)(0x00);//identifier distribution (0=CANaerospace distribution)
00188     data[7] = (char)(0x00);//header type  (0 = CANaerospace standard header)
00189     if(can1.write(CANMessage(129,data,8))) {
00190             printf("ids responce from %d is sent",node_ID);
00191         }  
00192 }
00193 /* nss dont request a response
00194 void nodespresponse::nss(unsigned long time){
00195     char data[8];
00196     char data1[4];
00197     char data2[4]
00198     char canmsg[8];
00199     data1[0] = (char)(node_ID);//node_id
00200     data1[1] = (char)(0x04);//data type:no data
00201     data1[2] = (char)(0x01);//service code 1
00202     data1[3] = (char)(0x00);//message code as in request 
00203     memcpy(data2,&time,4);
00204     memcpy(canmsg, data1, 4);
00205     memcpy(canmsg+4, data2, 8);
00206     can1.write(CANMessage(1124,canmsg,8));
00207 }
00208 */
00209 
00210 
00211 /////////////////////////data download service
00212 //////first response a start download request message
00213 void nodespresponse::dds1(){
00214     char data[8];
00215     data[0] = (char)(node_ID);//node_id
00216     data[1] = (char)(0x03);//data type:long
00217     data[2] = (char)(0x02);//service code 2
00218     data[3] = (char)(0x00);//as in request
00219     data[4] = (char)(0x00);/////-2=invalid//-1=abort//0=xoff//1=xon
00220     data[5] = (char)(0x00);
00221     data[6] = (char)(0x00);
00222     data[7] = (char)(0x00);
00223 }
00224 ////////second response the message
00225 void nodespresponse::dds2(){
00226     char data[8];
00227     data[0] = (char)(node_ID);//node_id
00228     data[1] = (char)(0x16);//data type:chksum
00229     data[2] = (char)(0x02);//service code 2
00230     data[3] = (char)(0x00);//last number
00231     data[4] = (char)(0x00);//checksum
00232     data[5] = (char)(0x00);
00233     data[6] = (char)(0x00);
00234     data[7] = (char)(0x00);
00235 }
00236 /////////////////////////data upload service
00237 //////first response a start download request message
00238 void nodespresponse::dus1(){
00239     char data[8];
00240     data[0] = (char)(node_ID);//node_id
00241     data[1] = (char)(0x15);//data type:MEMID
00242     data[2] = (char)(0x03);//service code 3
00243     data[3] = (char)(0x00);
00244     data[4] = (char)(0x00);
00245     data[5] = (char)(0x00);
00246     data[6] = (char)(0x00);
00247     data[7] = (char)(0x00);
00248 }
00249 ////////second response the message
00250 void nodespresponse::dus2(){
00251     char data[8];
00252     data[0] = (char)(node_ID);//node_id
00253     data[1] = (char)(0x15);//data type:MEMID
00254     data[2] = (char)(0x03);//service code 3
00255     data[3] = (char)(0x00); 
00256     data[4] = (char)(0x00);
00257     data[5] = (char)(0x00);
00258     data[6] = (char)(0x00);
00259     data[7] = (char)(0x00);
00260 }
00261 
00262 ////////////simulation control service
00263 
00264 void nodespresponse::scs(){
00265     char data[8];
00266     data[0] = (char)(node_ID);//node_id
00267     data[1] = (char)(0x00);//data type:??????????? user defined
00268     data[2] = (char)(0x04);//service code 4
00269     data[3] = (char)(0x00);//message code:???????????? 
00270     data[4] = (char)(0x00);/////user defined
00271     data[5] = (char)(0x00);
00272     data[6] = (char)(0x00);
00273     data[7] = (char)(0x00);
00274 }
00275 
00276 //////Transmission Interval Service
00277 
00278 void nodespresponse::tis(){
00279     char data[8];
00280     data[0] = (char)(node_ID);//node_id
00281     data[1] = (char)(0x00);//data type:no data
00282     data[2] = (char)(0x05);//service code 
00283     data[3] = (char)(0x00);//message code:0=ok//-6 =CAN identifier or transmission rate out of range
00284     data[4] = (char)(0x00);/////na
00285     data[5] = (char)(0x00);/////na
00286     data[6] = (char)(0x00);////na
00287     data[7] = (char)(0x00);////na
00288 }
00289 //////FLASH Programming Service
00290 
00291 void nodespresponse::fps(){
00292     char data[8];
00293     data[0] = (char)(node_ID);//node_id
00294     data[1] = (char)(0x00);//data type:nodata
00295     data[2] = (char)(0x06);//service code 6
00296     data[3] = (char)(0x00);//message code:0 = OK ///-3 = invalid security code
00297     data[4] = (char)(0x00);/////na
00298     data[5] = (char)(0x00);
00299     data[6] = (char)(0x00);
00300     data[7] = (char)(0x00);
00301 }
00302 
00303 /* no response
00304 //////State Transmission Service
00305 
00306 void nodespresponse::sts(){
00307     char data[8];
00308     data[0] = (char)(node_ID);//node_id
00309     data[1] = (char)(0x00);//data type:nodata
00310     data[2] = (char)(0x07);//service code 7
00311     data[3] = (char)(0x00);//message code:0
00312     data[4] = (char)(0x00);/////na
00313     data[5] = (char)(0x00);
00314     data[6] = (char)(0x00);
00315     data[7] = (char)(0x00);
00316 }
00317 */
00318 
00319 //////Filter setting services
00320 
00321 void nodespresponse::fss(){
00322     char data[8];
00323     data[0] = (char)(node_ID);//node_id
00324     data[1] = (char)(0x00);//data type:no data
00325     data[2] = (char)(0x08);//service code 8
00326     data[3] = (char)(0x00);//message code:0 = OK -6 = limit frequency or filter number out of rang
00327     data[4] = (char)(0x00);/////na
00328     data[5] = (char)(0x00);
00329     data[6] = (char)(0x00);
00330     data[7] = (char)(0x00);
00331 }
00332 
00333 
00334 
00335 //////Test Control Service 
00336 
00337 void nodespresponse::tcs(){
00338     char data[8];
00339     data[0] = (char)(node_ID);//node_id
00340     data[1] = (char)(0x00);//data type:user defined
00341     data[2] = (char)(0x09);//service code 9
00342     data[3] = (char)(0x00);//message code:user defined
00343     data[4] = (char)(0x00);/////user defined
00344     data[5] = (char)(0x00);
00345     data[6] = (char)(0x00);
00346     data[7] = (char)(0x00);
00347 }
00348 
00349 
00350 
00351 //////Baudrate Setting Service
00352 
00353 void nodespresponse::bss(){
00354     char data[8];
00355     data[0] = (char)(node_ID);//node_id
00356     data[1] = (char)(0x06);//data type:short
00357     data[2] = (char)(0xA);//service code 10
00358     data[3] = (char)(0x00);//message code:-1 = baudrate code unknown
00359     data[4] = (char)(0x00);/////na
00360     data[5] = (char)(0x00);/////na
00361     data[6] = (char)(0x00);///na
00362     data[7] = (char)(0x00);////na
00363 }
00364 
00365 
00366 //////Node-ID Setting Service
00367 
00368 void nodespresponse::nis(){
00369     char data[8];
00370     data[0] = (char)(node_ID);//node_id
00371     data[1] = (char)(0x00);//data type:no data
00372     data[2] = (char)(0xB);//service code 11
00373     data[3] = (char)(0x00);//message code:0 = OK -6 = node-ID out of range
00374     data[4] = (char)(0x00);/////na
00375     data[5] = (char)(0x00);
00376     data[6] = (char)(0x00);
00377     data[7] = (char)(0x00);
00378 }
00379 
00380 ////// Module Information Service 
00381 
00382 void nodespresponse::mis(){
00383     char data[8];
00384     data[0] = (char)(node_ID);//node_id
00385     data[1] = (char)(0x00);//data type:user defined
00386     data[2] = (char)(0xC);//service code 12
00387     data[3] = (char)(0x00);//message code:user defined
00388     data[4] = (char)(0x00);/////user defined
00389     data[5] = (char)(0x00);
00390     data[6] = (char)(0x00);
00391     data[7] = (char)(0x00);
00392 }
00393 //////  Module Configuration Service
00394 
00395 void nodespresponse::mcs(){
00396     char data[8];
00397     data[0] = (char)(node_ID);//node_id
00398     data[1] = (char)(0x00);//data type:user defined
00399     data[2] = (char)(0xD);//service code 13
00400     data[3] = (char)(0x00);//message code:user defined
00401     data[4] = (char)(0x00);/////user defined
00402     data[5] = (char)(0x00);
00403     data[6] = (char)(0x00);
00404     data[7] = (char)(0x00);
00405 }
00406 ////// CAN Identifier Setting Service
00407 
00408 void nodespresponse::css(){
00409     char data[8];
00410     data[0] = (char)(node_ID);//node_id
00411     data[1] = (char)(0x00);//data type:no data
00412     data[2] = (char)(0xE);//service code 14
00413     data[3] = (char)(0x00);//message code:0 = OK -6 = message number or CAN identifier out of range
00414     data[4] = (char)(0x00);/////na
00415     data[5] = (char)(0x00);/////na
00416     data[6] = (char)(0x00);////na
00417     data[7] = (char)(0x00);///na
00418 }
00419 
00420 //////CANaerospace Identifier Distribution Setting Service
00421 
00422 void nodespresponse::dss(){
00423     char data[8];
00424     data[0] = (char)(node_ID);//node_id
00425     data[1] = (char)(0x00);//data type:no data
00426     data[2] = (char)(0xF);//service code 15
00427     data[3] = (char)(0x00);//message code:0= ok //-6= distribution id out of range
00428     data[4] = (char)(0x00);/////na
00429     data[5] = (char)(0x00);
00430     data[6] = (char)(0x00);
00431     data[7] = (char)(0x00);
00432 }