Diff: main.cpp

Revision:

0:f9b3d6a82c03

diff -r 000000000000 -r f9b3d6a82c03 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Feb 19 10:02:29 2019 +0000
@@ -0,0 +1,183 @@
+// CAN_MONITOR with filtering  2012/03/21 ym1784
+ 
+#include "mbed.h"
+#include "CAN.h"
+ 
+Serial pc(USBTX, USBRX); // tx, rx
+DigitalOut led2(LED2);
+DigitalOut led1(LED1); 
+ // Tiker for sending messges 
+ Ticker ticker;
+ 
+ int CAN_ID = 1;
+ char counter = 1;
+ 
+// CAN2 on mbed pins 29(CAN_TXD) and 30(CAN_RXD) using MCP2551.
+CAN can2(p30, p29);
+CAN can1(p9, p10);
+/*--------------------------------------------
+  setup acceptance filter for CAN controller 2
+  original http://www.dragonwake.com/download/LPC1768/Example/CAN/CAN.c
+  simplified for CAN2 interface and std id (11 bit) only
+ *--------------------------------------------*/
+void 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 = 0x00000000;                  /* Use acceptance filter */
+} // CAN2_wrFilter
+
+void send() {
+    pc.printf("send()\n\r");
+    if(can1.write(CANMessage(CAN_ID, &counter, 1))) {
+        pc.printf("wloop()\n\r");
+        pc.printf("Message sent: %d with can ID: %d \n\r", counter, CAN_ID);
+        if (CAN_ID < 3)
+        {
+            CAN_ID ++;   
+        }
+        else
+        {
+            CAN_ID = 1;
+        }
+    } 
+    led1 = !led1;
+}
+ 
+int main() {
+    pc.baud(921600);
+    pc.printf("CAN_MONITOR 921600 bps\r\n");
+ 
+    // 500kbit/s
+    can1.frequency(500000);
+    can2.frequency(500000);
+    CANMessage can_MsgRx;
+ 
+    CAN2_wrFilter(0x178);
+    CAN2_wrFilter(0x1C4);
+    CAN2_wrFilter(0x245);
+    CAN2_wrFilter(0x3D3);
+    CAN2_wrFilter(0x498);
+    CAN2_wrFilter(0x4A6);
+    //ticker.attach(&send, 1);
+    int Yaw_Byte1, Yaw_Byte2, Yaw, Y_Acc_Byte1, Y_Acc_Byte2, Y_Acc, Roll_Byte1, Roll_Byte2, Roll, X_Acc_Byte1, X_Acc_Byte2, X_Acc, Z_Acc_Byte1, Z_Acc_Byte2, Z_Acc;
+ 
+    while (1) {
+        // send received messages to the pc via serial line
+        if (can2.read(can_MsgRx)) {
+            pc.printf("Can ID: %d, Message Length: %d", can_MsgRx.id, can_MsgRx.len);
+            if(can_MsgRx.id == 372)
+                {
+                    Yaw_Byte1 = can_MsgRx.data[0];
+                    Yaw_Byte2 = can_MsgRx.data[1]<<8; // shift right to normailse the second byte of data, same with others 
+                    Y_Acc_Byte1 = can_MsgRx.data[4];
+                    Y_Acc_Byte2 = can_MsgRx.data[5]<<8; //^
+                }
+                if(can_MsgRx.id == 376)
+                {
+                    Roll_Byte1 = can_MsgRx.data[0];
+                    Roll_Byte2 = can_MsgRx.data[1]<<8; //^
+                    X_Acc_Byte1 = can_MsgRx.data[4];
+                    X_Acc_Byte2 = can_MsgRx.data[5]<<8; //^
+                }
+                if(can_MsgRx.id == 380)
+                {
+                    Z_Acc_Byte1 = can_MsgRx.data[4];
+                    Z_Acc_Byte2 = can_MsgRx.data[5]<<8; //^
+                }
+                //Calculate actual data by adding both bytes and the offset anf then normalising
+                Yaw = (Yaw_Byte1 + Yaw_Byte2 - 32768)*0.005;
+                Y_Acc = (Y_Acc_Byte1 + Y_Acc_Byte2 - 32768)*0.0001274;
+                Roll = (Roll_Byte1 + Roll_Byte2 - 32768)*0.005;
+                X_Acc = (X_Acc_Byte1 + X_Acc_Byte2 - 32768)*0.0001274;
+                Z_Acc = (Z_Acc_Byte1 + Z_Acc_Byte2 - 32768)*0.0001274;
+               
+                // show the id of the messege being read
+                // print all of the data to pc serial
+                pc.printf("Yaw: %d\n\r", Yaw); 
+                pc.printf("Roll: %d\n\r", Roll); 
+                pc.printf("Y_Acc: %d\n\r", Y_Acc); 
+                pc.printf("X_Acc: %d\n\r", X_Acc); 
+                pc.printf("Z_Acc: %d\n\r", Z_Acc);   
+            pc.printf("\r\n");
+            // toggle led2
+            led2 = !led2;
+        } // if
+    } // while
+} // main
\ No newline at end of file