Changeset 1727 for trunk/MagicSoft/Cosy/candrv/canopen.cc

Timestamp:

01/23/03 13:32:58 (22 years ago)

Author:

tbretz

Message:

*** empty log message ***

File:

• trunk/MagicSoft/Cosy/candrv/canopen.cc (modified) (11 diffs)

trunk/MagicSoft/Cosy/candrv/canopen.cc

@@ -16 +16 @@
 !
 !
-!   Author(s): Thomas Bretz <mailto:tbretz@uni-sw.gwdg.de>, 2001
-!
-!   Copyright: MAGIC Software Development, 2000-2001
+!   Author(s): Thomas Bretz <mailto:tbretz@astro.uni-wuerzburg.de>, 2003
+!
+!   Copyright: MAGIC Software Development, 2000-2003
 !
 !
@@ -84 +84 @@
 {
     const WORD_t fcode = cobid >> 7;
+    const BYTE_t  node = cobid & 0x1f;
 
     switch (fcode)
@@ -94 +95 @@
         return;
 
-    case kSYNC:
-        cout << "Sync" << endl;
+    case kEMERGENCY: // also case kSYNC:
+        if (cobid==0)
+            cout << "Sync" << endl;
+        else
+        {
+            cout << "EMERGENCY Node #" << dec << (int)data[1] << endl;
+            HandleEmergency(node, tv);
+        }
+        return;
+
+    case kNodeguard:
+        //cout << "Nodeguard Node #" << dec << (int)node << endl;
+        HandleNodeguard(node, tv);
         return;
 
     case kSDO_RX:
         {
-            const BYTE_t  node   = cobid & 0x1f;
             const BYTE_t  cmd    = data[0];
             const LWORD_t dat    = data[4] | (data[5]<<8) | (data[6]<<16) | (data[7]<<24);
@@ -114 +125 @@
     case kPDO1_TX:
         {
-            const BYTE_t node = cobid & 0x1f;
             HandlePDO1(node, data, tv);
             pthread_cond_broadcast(&fPdoCond[node-1][0]);
@@ -122 +132 @@
     case kPDO2_TX:
         {
-            const BYTE_t node = cobid & 0x1f;
             HandlePDO2(node, data, tv);
             pthread_cond_broadcast(&fPdoCond[node-1][1]);
@@ -130 +139 @@
     case kPDO3_TX:
         {
-            const BYTE_t node = cobid & 0x1f;
             HandlePDO3(node, data, tv);
             pthread_cond_broadcast(&fPdoCond[node-1][2]);
@@ -138 +146 @@
     case kPDO4_TX:
         {
-            const BYTE_t node = cobid & 0x1f;
             HandlePDO4(node, data, tv);
             pthread_cond_broadcast(&fPdoCond[node-1][3]);
@@ -145 +152 @@
     }
 
-    const BYTE_t node = cobid & 0x1f;
-    cout << "Function Code: 0x" << hex << fcode << "  Node: " << dec << (int)node << endl;
+    cout << "CanOpen::HandleCanMessage - Unhandled Message: Function Code=0x" << hex << fcode << "  Node #" << dec << (int)node << endl;
 }
 
@@ -168 +174 @@
 {
     EnableCanMsg(node, kEMERGENCY);
+}
+
+// --------------------------------------------------------------------------
+//
+// Enables Nodeguard messages for a given node
+//
+void CanOpen::EnableNodeguard(BYTE_t node)
+{
+    EnableCanMsg(node, kNodeguard);
 }
 
@@ -372 +387 @@
 // --------------------------------------------------------------------------
 //
-// Send an NMT Message to the given node with command cmd
+// Send a NMT Message to the given node with command cmd
 //
 void CanOpen::SendNMT(BYTE_t node, BYTE_t cmd)
@@ -381 +396 @@
 // --------------------------------------------------------------------------
 //
+// Send a Nodeguard Message to the given node with command cmd
+//
+void CanOpen::SendNodeguard(BYTE_t node)
+{
+    BYTE_t msg[8] = { 0, 0, 0, 0, 0, 0, 0, 0 };
+    SendCanFrame(CobId(node, kNodeguard), msg, 1);
+}
+
+// --------------------------------------------------------------------------
+//
 // Decodes node and function code into a CobId
 //