Index: trunk/MagicSoft/Cosy/Changelog
===================================================================
--- trunk/MagicSoft/Cosy/Changelog	(revision 6922)
+++ trunk/MagicSoft/Cosy/Changelog	(revision 6923)
@@ -1,3 +1,18 @@
                                                                   -*-*- END -*-*-
+
+ 2005/04/11 - Thomas Bretz
+
+   * main/MTracking.cc:
+     - allow to send velocity and request position twice
+     - enhanced some debugging output
+
+   * candrv/network.[h,cc]:
+     - implemented PrintError
+
+   * candrv/nodedrv.[h,cc]:
+     - implemented new argument (zombie) to WaitForSdo which allows
+       to mark a missing CANbus message as non-critical
+
+
 
  2005/03/21 - Thomas Bretz
Index: trunk/MagicSoft/Cosy/candrv/network.cc
===================================================================
--- trunk/MagicSoft/Cosy/candrv/network.cc	(revision 6922)
+++ trunk/MagicSoft/Cosy/candrv/network.cc	(revision 6923)
@@ -278,4 +278,27 @@
 // --------------------------------------------------------------------------
 //
+// Print all errors which are currently set
+//
+void Network::PrintError() const
+{
+    for (int i=0; i<32; i++)
+    {
+        if (!fNodes[i])
+            continue;
+
+        if (!fNodes[i]->HasError())
+            continue;
+
+        lout << "- Node #" << dec << i << " '" << fNodes[i]->GetNodeName() << "' ";
+
+        if (fNodes[i]->GetError() <= 0)
+            lout << "Error occured." << endl;
+        else
+            lout << "has error #" << fNodes[i]->GetError() << endl;
+    }
+}
+
+// --------------------------------------------------------------------------
+//
 // returns true if one of the nodes has the error-flag set (HasError).
 //
@@ -354,3 +377,2 @@
             fNodes[i]->CheckConnection();
 }
-
Index: trunk/MagicSoft/Cosy/candrv/network.h
===================================================================
--- trunk/MagicSoft/Cosy/candrv/network.h	(revision 6922)
+++ trunk/MagicSoft/Cosy/candrv/network.h	(revision 6923)
@@ -37,4 +37,6 @@
     virtual void Stop();
 
+    void PrintError() const;
+
     bool HasError() const;
     bool HasZombie() const;
Index: trunk/MagicSoft/Cosy/candrv/nodedrv.cc
===================================================================
--- trunk/MagicSoft/Cosy/candrv/nodedrv.cc	(revision 6922)
+++ trunk/MagicSoft/Cosy/candrv/nodedrv.cc	(revision 6923)
@@ -416,21 +416,14 @@
 // queue and no waiting is done, false is returned..
 //
-bool NodeDrv::WaitForSdo(WORD_t idx, BYTE_t subidx, WORDS_t timeout)
+bool NodeDrv::WaitForSdo(WORD_t idx, BYTE_t subidx, WORDS_t timeout, bool zombie)
 {
     bool rc = fNetwork->WaitForSdo(fId, idx, subidx, fIsZombie?-1:timeout);
-
-    if (!rc)
-    {
-        lout << "NodeDrv::WaitForSdo: 0x" << hex << idx << "/" << dec << (int)subidx << " " << GetNodeName() << " --> ZOMBIE!" << endl;
+    if (rc)
+        return true;
+
+    lout << " + " << GetNodeName() << ": NodeDrv::WaitForSdo: 0x" << hex << idx << "/" << dec << (int)subidx << " --> ZOMBIE! " << MTime() << endl;
+    if (zombie)
         SetZombie();
-    }
-/*
-    if (HasError())
-    {
-        lout << "NodeDrv::WaitForSdo: HasError 0x" << hex << idx << "/" << dec << (int)subidx << " " << GetNodeName() << " --> ZOMBIE!" << endl;
-        fIsZombie = kTRUE;
-    }
-*/
-    return fIsZombie ? false : rc;
+    return false;
 }
 
Index: trunk/MagicSoft/Cosy/candrv/nodedrv.h
===================================================================
--- trunk/MagicSoft/Cosy/candrv/nodedrv.h	(revision 6922)
+++ trunk/MagicSoft/Cosy/candrv/nodedrv.h	(revision 6923)
@@ -106,5 +106,5 @@
 
     // void WaitForSdos();
-    bool WaitForSdo(WORD_t idx, BYTE_t subidx=0, WORDS_t timeout=500);
+    bool WaitForSdo(WORD_t idx, BYTE_t subidx=0, WORDS_t timeout=500, bool zombie=true);
 
     void EnableCanMsg(BYTE_t fcode);
Index: trunk/MagicSoft/Cosy/main/MTracking.cc
===================================================================
--- trunk/MagicSoft/Cosy/main/MTracking.cc	(revision 6922)
+++ trunk/MagicSoft/Cosy/main/MTracking.cc	(revision 6923)
@@ -22,21 +22,26 @@
 bool MTracking::RequestRePos()
 {
-    //
-    // Send request
-    //
-    fCosy->fMac2->RequestSDO(0x6004);
-    fCosy->fMac1->RequestSDO(0x6004);
-
-    //
-    // Wait until the objects are received.
-    //
-    fCosy->fMac2->WaitForSdo(0x6004);
-    fCosy->fMac1->WaitForSdo(0x6004);
-
-    //
-    // If waiting was not interrupted everything is ok. return.
-    //
-    if (!Break())
-        return true;
+    for (int i=0; i<2; i++)
+    {
+        //
+        // Send request
+        //
+        fCosy->fMac2->RequestSDO(0x6004);
+        fCosy->fMac1->RequestSDO(0x6004);
+
+        //
+        // Wait until the objects are received.
+        //
+        fCosy->fMac2->WaitForSdo(0x6004, 0, 500, i>0);
+        fCosy->fMac1->WaitForSdo(0x6004, 0, 500, i>0);
+
+        //
+        // If waiting was not interrupted everything is ok. return.
+        //
+        if (!Break())
+            return true;
+
+        fCosy->PrintError();
+    }
 
     //
@@ -48,4 +53,45 @@
 
     return false;
+}
+
+// --------------------------------------------------------------------------
+//
+// Sets the tracking velocity
+//
+// The velocities are given in a ZdAz object in re/min. Return kTRUE
+// in case of success, kFALSE in case of failure.
+//
+Bool_t MTracking::SetVelocity(const ZdAz &v)
+{
+    for (int i=0; i<2; i++)
+    {
+        //
+        // Send the new velocities for both axes.
+        //
+        fCosy->fMac2->SendSDO(0x3006, 1, (LWORD_t)v.Zd());  // SetRpmVelocity [re/min]
+        fCosy->fMac1->SendSDO(0x3006, 1, (LWORD_t)v.Az());  // SetRpmVelocity [re/min]
+
+        //
+        // Wait for the objects to be acknoledged.
+        //
+        fCosy->fMac2->WaitForSdo(0x3006, 1, 500, i>0);
+        fCosy->fMac1->WaitForSdo(0x3006, 1, 500, i>0);
+
+        //
+        // If the waiting for the objects wasn't interrupted return kTRUE
+        //
+        if (!Break())
+            return kTRUE;
+
+        fCosy->PrintError();
+    }
+
+    //
+    // print a message if the interruption was due to a Can-node Error
+    //
+    if (fCosy->HasError())
+        lout << "Error while setting tracking velocity (SDO #3006)" << endl;
+
+    return kFALSE;
 }
 
@@ -189,40 +235,4 @@
 }
 
-// --------------------------------------------------------------------------
-//
-// Sets the tracking velocity
-//
-// The velocities are given in a ZdAz object in re/min. Return kTRUE
-// in case of success, kFALSE in case of failure.
-//
-Bool_t MTracking::SetVelocity(const ZdAz &v)
-{
-    //
-    // Send the new velocities for both axes.
-    //
-    fCosy->fMac2->SendSDO(0x3006, 1, (LWORD_t)v.Zd());  // SetRpmVelocity [re/min]
-    fCosy->fMac1->SendSDO(0x3006, 1, (LWORD_t)v.Az());  // SetRpmVelocity [re/min]
-
-    //
-    // Wait for the objects to be acknoledged.
-    //
-    fCosy->fMac2->WaitForSdo(0x3006, 1);
-    fCosy->fMac1->WaitForSdo(0x3006, 1);
-
-    //
-    // If the waiting for the objects wasn't interrupted return kTRUE
-    //
-    if (!Break())
-        return kTRUE;
-
-    //
-    // print a message if the interruption was due to a Can-node Error
-    //
-    if (fCosy->HasError())
-        lout << "Error while setting tracking velocity (SDO #3006)" << endl;
-
-    return kFALSE;
-}
-
 // --------------------------------------------------------------------
 //
@@ -237,8 +247,23 @@
     const XY re = fCosy->kGearTot/fCosy->kResSE; //[re/se]
 
+    Int_t pzd1 = fCosy->fZd1->GetPos();
+    Int_t pzd2 = fCosy->fZd2->GetPos();
+    Int_t paz  = fCosy->fAz->GetPos();
+
+    if (fCosy->fZd1->DirHasChanged() != fCosy->fZd2->DirHasChanged())
+    {
+        pzd1 -= fCosy->fZd1->GetDirection();
+        pzd2 -= fCosy->fZd2->GetDirection();
+    }
+    else
+    {
+        fCosy->fZd1->ResetDirHasChanged();
+        fCosy->fZd2->ResetDirHasChanged();
+    }
+
     // Get current shaftencoder position of the telescope
-    Double_t seposzd1 = ((fCosy->fZd1->GetPos()+8192)%16384)*re.X();
-    Double_t seposzd2 = ((fCosy->fZd2->GetPos()+8192)%16384)*re.X();
-    Double_t seposaz  =   fCosy->fAz->GetPos() *re.Y();
+    Double_t seposzd1 = ((pzd1+8192)%16384)*re.X();
+    Double_t seposzd2 = ((pzd2+8192)%16384)*re.X();
+    Double_t seposaz  =   paz              *re.Y();
 
     // distance between (To+dt) and To [re]
@@ -503,5 +528,5 @@
         return (void*)3;
 
-    lout << "- Tracking Thread started..." << endl;
+    lout << "- Tracking Thread started (" << MTime() << ")" << endl;
 
     //const XY re2se = fCosy->kGearTot/fCosy->kResSE; //[re/se]
@@ -590,6 +615,5 @@
     }
 
-    lout << "- Tracking Thread done." << endl;
-
+    lout << "- Tracking Thread done. (" << MTIme() << ")" << endl;
     return 0;
 }