Index: trunk/MagicSoft/Cosy/candrv/canopen.cc
===================================================================
--- trunk/MagicSoft/Cosy/candrv/canopen.cc	(revision 2518)
+++ trunk/MagicSoft/Cosy/candrv/canopen.cc	(revision 8813)
@@ -18,5 +18,5 @@
 !   Author(s): Thomas Bretz <mailto:tbretz@astro.uni-wuerzburg.de>, 2003
 !
-!   Copyright: MAGIC Software Development, 2000-2003
+!   Copyright: MAGIC Software Development, 2000-2008
 !
 !
@@ -32,6 +32,8 @@
 #include "canopen.h"
 
-#include <iostream> // cout
-#include <iomanip>  // setw, setfill
+#include "MLog.h"
+#include "MLogManip.h"
+
+#include "interface.h"
 
 ClassImp(CanOpen);
@@ -44,7 +46,7 @@
 // PDO combinations
 //
-CanOpen::CanOpen(const char *dev, const int baud, MLog &out) : VmodIcan(dev, baud, out)
-{
-    lout << "- CanOpen initialized." << endl;
+CanOpen::CanOpen() : fInterface(0)
+{
+    gLog << inf << "- CanOpen initialized." << endl;
 }
 
@@ -55,10 +57,30 @@
 CanOpen::~CanOpen()
 {
-    lout << "- CanOpen destroyed." << endl;
-}
-
-// --------------------------------------------------------------------------
-//
-// This overloads VmodIcan::HandleCanMessage. It is called if a can
+    gLog << inf << "- CanOpen destroyed." << endl;
+}
+
+// --------------------------------------------------------------------------
+//
+// Start the interface
+//
+void CanOpen::Start()
+{
+    if (fInterface)
+        fInterface->Start();
+}
+
+// --------------------------------------------------------------------------
+//
+// Stop the interface
+//
+void CanOpen::Stop()
+{
+    if (fInterface)
+        fInterface->Stop();
+}
+
+// --------------------------------------------------------------------------
+//
+// This overloads Interface::HandleCanMessage. It is called if a can
 // message was received with all message relevant data (COBId, data, time)
 // It distributes the data depending on its function code to several
@@ -69,5 +91,5 @@
 //  HandlePDO1/2/3/4:handles received PDOs
 //
-void CanOpen::HandleCanMessage(WORD_t cobid, BYTE_t *data, timeval_t *tv)
+void CanOpen::HandleCanMessage(WORD_t cobid, const BYTE_t *data, const timeval_t &tv)
 {
     const WORD_t fcode = cobid >> 7;
@@ -105,4 +127,5 @@
             const WORD_t  subidx = data[3];
 
+            cout << "SND NODE: " << (int)node << " " << flush;
             HandleSDO(node, cmd, idx, subidx, dat, tv);
 
@@ -145,4 +168,161 @@
 // --------------------------------------------------------------------------
 //
+// Does a basic message processing and hadles the so called command (cmd)
+// stamp of the message. This is some kind of message type which is send
+// by the can interface
+//
+void CanOpen::HandleMessage(const Message &msg, const timeval_t &tv)
+{
+    //
+    // Decode message
+    //
+    const WORD_t desc  = msg.data[2]<<8 | msg.data[3];
+    const BYTE_t rtr   = (desc>>4)&1;
+    const BYTE_t len   = desc&0xf;
+    const WORD_t cobid = desc>>5;
+
+    switch (msg.cmd) // FROM mican.h
+    {
+    case M_MSG_LOST:
+        cout << "Interface reports: " << dec << (int)msg.data[0] << " msg(s) lost!" << endl;
+        return;
+
+    case M_BCAN_TX_con:  /* confirm (+/-) transmission */
+        cout << "Interface reports: CTXcon=0x35" << endl;
+        cout << "This normally means, that the transmission of the following CAN frame failed:" << hex << endl;
+        cout << "Descr: 0x" << cobid << dec;
+        cout << "  Rtr: "   << (rtr?"Yes":"No");
+        cout << "  Len: "   << (int)len << endl;
+        return;
+
+    case M_BCAN_EVENT_ind:
+        cout << "Interface reports: CEVTind=0x37: " << hex;
+        switch (msg.data[0]) // error indicator
+        {
+        case 0x01:
+            cout << "Error interrup occured" << endl;
+            cout << "This means noisy network normally. Please check the bus termination." << endl;
+            switch (msg.data[1]) // msg type (board depending)
+            {
+            case 2: // SJA1000
+                cout << dec;
+                cout << "ModeReg=" << (int)msg.data[2] << ", ";
+                cout << "StatReg=" << (int)msg.data[3] << ", ";
+                cout << "RxErrCnt=" << (int)msg.data[4] << ", ";
+                cout << "TxErrCnt=" << (int)msg.data[5] << endl;
+            }
+            //FIXME? TerminateApp();
+            return;
+        case 0x02:
+            cout << "Overrun interrup occured" << endl;
+            return;
+        case 0x04:
+            cout << "Interrupts lost" << endl;
+            return;
+        case 0x08:
+            cout << "Send queue full" << endl;
+            return;
+        case 0x10:
+            cout << "CANbus bus-error" << endl;
+            return;
+        }
+        return;
+    case M_BCAN_RX_ind:
+        //
+        // Message is a message from the Can bus
+        //
+        //cout << "HandleCanMessage " << cobid << endl;
+        HandleCanMessage(cobid, &msg.data[4], tv);
+        return;
+    }
+
+    //
+    // Nothing of the above happened
+    //
+    cout << hex;
+    cout << "Cmd=0x"    << (int)msg.cmd << ":  ";
+    cout << "Descr: 0x" << cobid << dec;
+    cout << "  Rtr: "   << (rtr?"Yes":"No");
+    cout << "  Len: "   << (int)len << endl;
+
+    cout << "As Raw Data:" << hex << setfill('0');
+    for (int i=0; i<msg.len; i++)
+        cout << " " << setw(2) << (int)(msg.data[i]) << flush;
+    cout << endl;
+}
+
+bool CanOpen::WaitForSdos(WORDS_t ms)
+{
+    // 0: unlimited
+    // <0: don't do a real wait.
+    if (ms<0)
+    {
+        fSdoList.DelAll();
+        return true;
+    }
+
+    MTimeout t(ms);
+    while (fSdoList.IsPending() &&
+           !StopWaitingForSDO() &&
+           !t.HasTimedOut())
+        usleep(1);
+
+    bool rc = true;
+    if (ms && t.HasTimedOut())
+    {
+        cout << "WaitForSdos timed out." << endl;
+        rc = false;
+    }
+    /*
+     if (StopWaitingForSDO())
+     {
+     cout << "WaitForSdos stopped." << endl;
+     rc = false;
+     }
+     */
+    if ((ms && t.HasTimedOut()) || StopWaitingForSDO())
+        fSdoList.DelAll();
+
+    return rc;
+}
+
+bool CanOpen::WaitForSdo(BYTE_t node, WORD_t idx, BYTE_t subidx, WORDS_t ms)
+{
+    // 0: unlimited
+    // <0: don't do a real wait.
+
+    if (ms<0)
+    {
+        fSdoList.Del(node, idx, subidx);
+        return true;
+    }
+
+    MTimeout t(ms);
+    while (fSdoList.IsPending(node, idx, subidx) &&
+           !StopWaitingForSDO() &&
+           !t.HasTimedOut())
+        usleep(1);
+
+    bool rc = true;
+    if (ms && t.HasTimedOut())
+    {
+        cout << "WaitForSdo Node #" << (int)node << " " << idx << "/" << (int)subidx << " timed out." << endl;
+        rc = false;
+    }
+    /*
+     if (StopWaitingForSDO())
+     {
+     cout << "WaitForSdo Node #" << (int)node << " " << idx << "/" << (int)subidx << " stopped." << endl;
+     rc = false;
+     }
+     */
+    if ((ms && t.HasTimedOut()) || StopWaitingForSDO())
+        fSdoList.Del(node, idx, subidx);
+
+    return rc;
+}
+
+// --------------------------------------------------------------------------
+//
 // Enables can messaged for a given node ID and function code.
 //
@@ -152,5 +332,6 @@
         return;
 
-    EnableCobId(CobId(node, fcode), flag);
+    if (fInterface)
+        fInterface->EnableCobId(CobId(node, fcode), flag);
 }
 
@@ -260,6 +441,6 @@
 //
 void CanOpen::SendPDO1(BYTE_t node,
-              BYTE_t m0=0, BYTE_t m1=0, BYTE_t m2=0, BYTE_t m3=0,
-              BYTE_t m4=0, BYTE_t m5=0, BYTE_t m6=0, BYTE_t m7=0)
+              BYTE_t m0, BYTE_t m1, BYTE_t m2, BYTE_t m3,
+              BYTE_t m4, BYTE_t m5, BYTE_t m6, BYTE_t m7)
 {
     BYTE_t msg[8] = { m0, m1, m2, m3, m4, m5, m6, m7 };
@@ -272,6 +453,6 @@
 //
 void CanOpen::SendPDO2(BYTE_t node,
-              BYTE_t m0=0, BYTE_t m1=0, BYTE_t m2=0, BYTE_t m3=0,
-              BYTE_t m4=0, BYTE_t m5=0, BYTE_t m6=0, BYTE_t m7=0)
+              BYTE_t m0, BYTE_t m1, BYTE_t m2, BYTE_t m3,
+              BYTE_t m4, BYTE_t m5, BYTE_t m6, BYTE_t m7)
 {
     BYTE_t msg[8] = { m0, m1, m2, m3, m4, m5, m6, m7 };
@@ -284,6 +465,6 @@
 //
 void CanOpen::SendPDO3(BYTE_t node,
-              BYTE_t m0=0, BYTE_t m1=0, BYTE_t m2=0, BYTE_t m3=0,
-              BYTE_t m4=0, BYTE_t m5=0, BYTE_t m6=0, BYTE_t m7=0)
+              BYTE_t m0, BYTE_t m1, BYTE_t m2, BYTE_t m3,
+              BYTE_t m4, BYTE_t m5, BYTE_t m6, BYTE_t m7)
 {
     BYTE_t msg[8] = { m0, m1, m2, m3, m4, m5, m6, m7 };
@@ -296,6 +477,6 @@
 //
 void CanOpen::SendPDO4(BYTE_t node,
-              BYTE_t m0=0, BYTE_t m1=0, BYTE_t m2=0, BYTE_t m3=0,
-              BYTE_t m4=0, BYTE_t m5=0, BYTE_t m6=0, BYTE_t m7=0)
+              BYTE_t m0, BYTE_t m1, BYTE_t m2, BYTE_t m3,
+              BYTE_t m4, BYTE_t m5, BYTE_t m6, BYTE_t m7)
 {
     BYTE_t msg[8] = { m0, m1, m2, m3, m4, m5, m6, m7 };
@@ -394,4 +575,46 @@
 // --------------------------------------------------------------------------
 //
+// This is IcSendReqBCAN from the Janz software
+//
+//  /*
+//   * IcSendReqBCAN - Send a CANbus message
+//   *
+//   * Issue request to send a CAN message. <Spec> controls whether to send with
+//   * or without spec/confirmation.
+//   * .CS
+//   *    spec     action
+//   *      0      send only
+//   *      1      send with confirmation to the host.
+//   *      2      send and echo message to the host.
+//   *      3      send and generate both echo and confirmation.
+//   * .CE
+//   *
+//   * SERVICE: CTXreq, CTXCreq, CTXEreq, CTXCEreq
+//   *
+//   * NOTE:
+//   * Raw ICANOS version of the firmware only.
+//   */
+//
+void CanOpen::SendCanFrame(WORD_t cobid, BYTE_t m[8], BYTE_t rtr)
+{
+    if (fInterface)
+        fInterface->SendCanFrame(cobid, m, rtr);
+}
+
+// --------------------------------------------------------------------------
+//
+// Sends a can frame with the given cobid and the given eight bytes
+// through the can network
+//
+void CanOpen::SendCanFrame(WORD_t cobid,
+                           BYTE_t m0, BYTE_t m1, BYTE_t m2, BYTE_t m3,
+                           BYTE_t m4, BYTE_t m5, BYTE_t m6, BYTE_t m7)
+{
+    BYTE_t msg[8] = { m0, m1, m2, m3, m4, m5, m6, m7 };
+    SendCanFrame(cobid, msg);
+}
+
+// --------------------------------------------------------------------------
+//
 // Decodes node and function code into a CobId
 //
