Changeset 1703 for trunk/MagicSoft/Cosy

Timestamp:

01/14/03 12:08:46 (22 years ago)

Author:

tbretz

Message:

*** empty log message ***

Location:

trunk/MagicSoft/Cosy

Files:

• Changelog (modified) (1 diff)
• candrv/canopen.h (modified) ( previous)
• candrv/network.cc (modified) (3 diffs)
• candrv/network.h (modified) (1 diff)
• candrv/nodedrv.cc (modified) (13 diffs)
• candrv/nodedrv.h (modified) (2 diffs)
• candrv/vmodican.cc (modified) (1 diff)
• cosy.cc (modified) (1 diff)
• devdrv/macs.cc (modified) (5 diffs)
• devdrv/macs.h (modified) (3 diffs)
• devdrv/shaftencoder.cc (modified) (6 diffs)
• devdrv/shaftencoder.h (modified) (2 diffs)
• main/MCosy.cc (modified) (42 diffs)
• main/MCosy.h (modified) (2 diffs)

trunk/MagicSoft/Cosy/Changelog

@@ -1 +1 @@
                                                                   -*-*- END -*-*-
+ 2003/01/14 - Thomas Bretz:
+
+   * cosy.cc:
+     - added output
+   
+   * candrv/network.[cc,h]:
+     - small change to Start
+     - added CheckConnections
+
+   * candrv/nodedrv.[h,cc]:
+     - new Init
+     - new CheckConnections
+     - replaced virtual InitDevice by a common function
+     - replaced virtual Reboot by a common function
+     - Don't send anything to a Zombie node
+     - Delete SDO from list in case of Zombie status instead of waiting
+     
+   * candrv/vmodican.cc:
+     - Don't terminate when having a noisy network 
+   
+   * devdrv/macs.[h,cc], devdrv/shaftencoder.[h,cc]:
+     - added fSoftVersion
+     - added SDO 0x100b
+     - moved init stuff from InitDevice to Init
+     - removed InitDevice and Reboot
+     - added CheckConnection
+
+   * devdrv/shaftencoder.[h,cc]:
+     - Don't display something when having Zombie status
+   
+   * main/MCosy.[h,cc]:
+     - reworked all Zombie-stuff
+     - implemented checking of network
+
+
 
  2003/01/13 - Thomas Bretz:

trunk/MagicSoft/Cosy/candrv/network.cc

@@ -237 +237 @@
             lout << "- Setting up Node #" << dec << i << " (";
             lout << fNodes[i]->GetNodeName() << ")" << endl;
-            fNodes[i]->InitDevice(this);
-            if (!fNodes[i]->IsZombieNode())
+            if (fNodes[i]->InitDevice(this))
                 fNodeInitialized[i] = TRUE;
-            /*else
-                fNodes[i]=NULL;*/
+            else
+                lout << "- " << fNodes[i]->GetNodeName() << ": InitDevice failed." << endl;
         }
     lout << "- All Nodes setup." << endl;
@@ -310 +309 @@
 bool Network::RebootZombies()
 {
+    bool rc = true;
+
     lout << "- Trying to reboot all Zombies..." << endl;
     for (int i=0; i<32; i++)
@@ -317 +318 @@
                 {
                     lout << "- Failed to reboot " << fNodes[i]->GetNodeName() << "." << endl;
-                    return false;
+                    rc = false;
                 }
 
-    lout << "- All Zombies rebooted." << endl;
-
-    return true;
-}
+    if (rc)
+        lout << "- All Zombies rebooted." << endl;
+
+    return rc;
+}
+
+// --------------------------------------------------------------------------
+//
+// Check the connections to all nodes. (This can also mean: Validate
+// the correct setup, etc)
+//
+void Network::CheckConnections()
+{
+    for (int i=0; i<32; i++)
+        if (fNodes[i])
+            fNodes[i]->CheckConnection();
+}
+

trunk/MagicSoft/Cosy/candrv/network.h

@@ -35 +35 @@
 
     bool RebootZombies();
+    void CheckConnections();
 
     ClassDef(Network, 0) // collection of nodes (nodedrv)

trunk/MagicSoft/Cosy/candrv/nodedrv.cc

@@ -30 +30 @@
 //
 // to be overloaded:
-//  virtual void InitDevice(Network *net)
+//  virtual void Init()
 //  virtual void StopDevice()
 //  virtual void HandleSDO(WORD_t idx, BYTE_t subidx, LWORD_t val, timeval_t *tv)
@@ -39 +39 @@
 //  virtual void HandlePDO3(BYTE_t *data, timeval_t *tv)
 //  virtual void HandlePDO4(BYTE_t *data, timeval_t *tv)
+//  virtual bool Reboot();
+//  virtual void CheckConnection();
 //
 ///////////////////////////////////////////////////////////////////////
@@ -56 +58 @@
 // and the node name. The name is a name for debug output.
 //
-NodeDrv::NodeDrv(BYTE_t nodeid, const char *name, MLog &out) : Log(out), fNetwork(NULL), fId(32), fError(0), fIsZombie(kFALSE)
+NodeDrv::NodeDrv(BYTE_t nodeid, const char *name, MLog &out) : Log(out), fNetwork(NULL), fId(32), fError(0), fIsZombie(kTRUE)
 {
     if (nodeid>0x1f)
@@ -93 +95 @@
 {
     fIsZombie = false;
-    return true;
+
+    Init();
+
+    return !fIsZombie;
 }
 
@@ -107 +112 @@
 //   SDO tx
 //
-void NodeDrv::InitDevice(Network *net)
+bool NodeDrv::InitDevice(Network *net)
 {
     fNetwork = net;
@@ -117 +122 @@
     EnableCanMsg(kSDO_RX);
     EnableCanMsg(kSDO_TX);
+
+    fIsZombie = kFALSE;
+
+    Init();
+
+    return !fIsZombie;
 }
 
@@ -165 +176 @@
 // A PDO is carrying up to eight bytes of information.
 //
-void NodeDrv::SendPDO1(BYTE_t data[8])
-{
-    fNetwork->SendPDO1(fId, data);
+// The message is not send if the node has the status Zombie.
+// In this case false is returned, otherwise true
+//
+bool NodeDrv::SendPDO1(BYTE_t data[8])
+{
+    if (!fIsZombie)
+        fNetwork->SendPDO1(fId, data);
+    return !fIsZombie;
 }
 
@@ -175 +191 @@
 // A PDO is carrying up to eight bytes of information.
 //
-void NodeDrv::SendPDO2(BYTE_t data[8])
-{
-    fNetwork->SendPDO2(fId, data);
+// The message is not send if the node has the status Zombie.
+// In this case false is returned, otherwise true
+//
+bool NodeDrv::SendPDO2(BYTE_t data[8])
+{
+    if (!fIsZombie)
+        fNetwork->SendPDO2(fId, data);
+    return !fIsZombie;
 }
 
@@ -185 +206 @@
 // A PDO is carrying up to eight bytes of information.
 //
-void NodeDrv::SendPDO1(BYTE_t m0=0, BYTE_t m1=0, BYTE_t m2=0, BYTE_t m3=0,
+// The message is not send if the node has the status Zombie.
+// In this case false is returned, otherwise true
+//
+bool NodeDrv::SendPDO1(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)
 {
-    fNetwork->SendPDO1(fId, m0, m1, m2, m3, m4, m5, m6, m7);
+    if (!fIsZombie)
+        fNetwork->SendPDO1(fId, m0, m1, m2, m3, m4, m5, m6, m7);
+    return !fIsZombie;
 }
 
@@ -196 +222 @@
 // A PDO is carrying up to eight bytes of information.
 //
-void NodeDrv::SendPDO2(BYTE_t m0=0, BYTE_t m1=0, BYTE_t m2=0, BYTE_t m3=0,
+// The message is not send if the node has the status Zombie.
+// In this case false is returned, otherwise true
+//
+bool NodeDrv::SendPDO2(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)
 {
-    fNetwork->SendPDO2(fId, m0, m1, m2, m3, m4, m5, m6, m7);
-}
-
-// --------------------------------------------------------------------------
-//
-// Sends the given SDO through the network to this device
-// An SDO message contains
-//  an address (this device)
-//  an index of the dictionary entry to address
-//  a subindex of this dictionary entry to access
-//  and a value to set for this dictionary entry
-//
-void NodeDrv::SendSDO(WORD_t idx, BYTE_t subidx, BYTE_t val, bool store)
-{
-    fNetwork->SendSDO(fId, idx, subidx, val, store);
-}
-
-// --------------------------------------------------------------------------
-//
-// Sends the given SDO through the network to this device
-// An SDO message contains
-//  an address (this device)
-//  an index of the dictionary entry to address
-//  a subindex of this dictionary entry to access
-//  and a value to set for this dictionary entry
-//
-void NodeDrv::SendSDO(WORD_t idx, BYTE_t subidx, WORD_t val, bool store)
-{
-    fNetwork->SendSDO(fId, idx, subidx, val, store);
-}
-
-// --------------------------------------------------------------------------
-//
-// Sends the given SDO through the network to this device
-// An SDO message contains
-//  an address (this device)
-//  an index of the dictionary entry to address
-//  a subindex of this dictionary entry to access
-//  and a value to set for this dictionary entry
-//
-void NodeDrv::SendSDO(WORD_t idx, BYTE_t subidx, LWORD_t val, bool store)
-{
-    fNetwork->SendSDO(fId, idx, subidx, val, store);
-}
-
-// --------------------------------------------------------------------------
-//
-// Sends the given SDO through the network to this device
-// An SDO message contains
-//  an address (this device)
-//  an index of the dictionary entry to address
-//  a subindex of this dictionary entry to access
-//  and a value to set for this dictionary entry
-//
-void NodeDrv::SendSDO(WORD_t idx, BYTE_t val)
-{
-    fNetwork->SendSDO(fId, idx, val, true);
-}
-
-// --------------------------------------------------------------------------
-//
-// Sends the given SDO through the network to this device
-// An SDO message contains
-//  an address (this device)
-//  an index of the dictionary entry to address
-//  a subindex of this dictionary entry to access
-//  and a value to set for this dictionary entry
-//
-void NodeDrv::SendSDO(WORD_t idx, WORD_t val)
-{
-    fNetwork->SendSDO(fId, idx, val, true);
-}
-
-// --------------------------------------------------------------------------
-//
-// Sends the given SDO through the network to this device
-// An SDO message contains
-//  an address (this device)
-//  an index of the dictionary entry to address
-//  a subindex of this dictionary entry to access
-//  and a value to set for this dictionary entry
-//
-void NodeDrv::SendSDO(WORD_t idx, LWORD_t val)
-{
-    fNetwork->SendSDO(fId, idx, val, true);
+    if (!fIsZombie)
+        fNetwork->SendPDO2(fId, m0, m1, m2, m3, m4, m5, m6, m7);
+    return !fIsZombie;
+}
+
+// --------------------------------------------------------------------------
+//
+// Sends the given SDO through the network to this device
+// An SDO message contains
+//  an address (this device)
+//  an index of the dictionary entry to address
+//  a subindex of this dictionary entry to access
+//  and a value to set for this dictionary entry
+//
+// The message is not send if the node has the status Zombie.
+// In this case false is returned, otherwise true
+//
+bool NodeDrv::SendSDO(WORD_t idx, BYTE_t subidx, BYTE_t val, bool store)
+{
+    if (!fIsZombie)
+        fNetwork->SendSDO(fId, idx, subidx, val, store);
+    return !fIsZombie;
+}
+
+// --------------------------------------------------------------------------
+//
+// Sends the given SDO through the network to this device
+// An SDO message contains
+//  an address (this device)
+//  an index of the dictionary entry to address
+//  a subindex of this dictionary entry to access
+//  and a value to set for this dictionary entry
+//
+// The message is not send if the node has the status Zombie.
+// In this case false is returned, otherwise true
+//
+bool NodeDrv::SendSDO(WORD_t idx, BYTE_t subidx, WORD_t val, bool store)
+{
+    if (!fIsZombie)
+        fNetwork->SendSDO(fId, idx, subidx, val, store);
+    return !fIsZombie;
+}
+
+// --------------------------------------------------------------------------
+//
+// Sends the given SDO through the network to this device
+// An SDO message contains
+//  an address (this device)
+//  an index of the dictionary entry to address
+//  a subindex of this dictionary entry to access
+//  and a value to set for this dictionary entry
+//
+// The message is not send if the node has the status Zombie.
+// In this case false is returned, otherwise true
+//
+bool NodeDrv::SendSDO(WORD_t idx, BYTE_t subidx, LWORD_t val, bool store)
+{
+    if (!fIsZombie)
+        fNetwork->SendSDO(fId, idx, subidx, val, store);
+    return !fIsZombie;
+}
+
+// --------------------------------------------------------------------------
+//
+// Sends the given SDO through the network to this device
+// An SDO message contains
+//  an address (this device)
+//  an index of the dictionary entry to address
+//  a subindex of this dictionary entry to access
+//  and a value to set for this dictionary entry
+//
+// The message is not send if the node has the status Zombie.
+// In this case false is returned, otherwise true
+//
+bool NodeDrv::SendSDO(WORD_t idx, BYTE_t val)
+{
+    if (!fIsZombie)
+        fNetwork->SendSDO(fId, idx, val, true);
+    return !fIsZombie;
+}
+
+// --------------------------------------------------------------------------
+//
+// Sends the given SDO through the network to this device
+// An SDO message contains
+//  an address (this device)
+//  an index of the dictionary entry to address
+//  a subindex of this dictionary entry to access
+//  and a value to set for this dictionary entry
+//
+// The message is not send if the node has the status Zombie.
+// In this case false is returned, otherwise true
+//
+bool NodeDrv::SendSDO(WORD_t idx, WORD_t val)
+{
+    if (!fIsZombie)
+        fNetwork->SendSDO(fId, idx, val, true);
+    return !fIsZombie;
+}
+
+// --------------------------------------------------------------------------
+//
+// Sends the given SDO through the network to this device
+// An SDO message contains
+//  an address (this device)
+//  an index of the dictionary entry to address
+//  a subindex of this dictionary entry to access
+//  and a value to set for this dictionary entry
+//
+// The message is not send if the node has the status Zombie.
+// In this case false is returned, otherwise true
+//
+bool NodeDrv::SendSDO(WORD_t idx, LWORD_t val)
+{
+    if (!fIsZombie)
+        fNetwork->SendSDO(fId, idx, val, true);
+    return !fIsZombie;
 }
 
@@ -294 +355 @@
 //  a subindex of this dictionary entry to access
 //
-void NodeDrv::RequestSDO(WORD_t idx, BYTE_t subidx)
-{
-    fNetwork->RequestSDO(fId, idx, subidx);
+// The message is not send if the node has the status Zombie.
+// In this case false is returned, otherwise true
+//
+bool NodeDrv::RequestSDO(WORD_t idx, BYTE_t subidx)
+{
+    if (!fIsZombie)
+        fNetwork->RequestSDO(fId, idx, subidx);
+    return !fIsZombie;
 }
 
@@ -303 +369 @@
 // Send an NMT message (command) to this device
 //
-void NodeDrv::SendNMT(BYTE_t cmd)
-{
-    fNetwork->SendNMT(fId, cmd);
+// The message is not send if the node has the status Zombie.
+// In this case false is returned, otherwise true
+//
+bool NodeDrv::SendNMT(BYTE_t cmd)
+{
+    if (!fIsZombie)
+        fNetwork->SendNMT(fId, cmd);
+    return !fIsZombie;
 }
 
@@ -323 +394 @@
 // You can stop waiting by StopWaitingForSDO.
 // Return false if waiting timed out.
+// If waiting timed out the node is set to status Zombie.
+//
+// If the node is already a zombie node, the message is deleted from the
+// queue and no waiting is done, false is returned..
 //
 bool NodeDrv::WaitForSdo(WORD_t idx, BYTE_t subidx, WORDS_t timeout)
 {
-    bool rc = fNetwork->WaitForSdo(fId, idx, subidx, timeout);
+    bool rc = fNetwork->WaitForSdo(fId, idx, subidx, fIsZombie?-1:timeout);
 
     if (!rc)
         fIsZombie = kTRUE;
 
-    return rc;
+    return fIsZombie ? false : rc;
 }
 

trunk/MagicSoft/Cosy/candrv/nodedrv.h

@@ -44 +44 @@
     Network    *GetNetwork()        { return fNetwork; }
 
-    virtual void InitDevice(Network *net);
+    virtual void Init() = 0;
+    virtual bool InitDevice(Network *net);
     virtual void StopDevice() = 0;
+    virtual bool Reboot();
+    virtual void CheckConnection() = 0;
 
     int  GetError() const { return fError; }
@@ -61 +64 @@
     virtual void HandlePDO4(BYTE_t *data, timeval_t *tv) {};
 
-    virtual bool Reboot();
-
-    void SendPDO1(BYTE_t data[8]);
-    void SendPDO2(BYTE_t data[8]);
-    void SendPDO1(BYTE_t m0=0, BYTE_t m1=0, BYTE_t m2=0, BYTE_t m3=0,
+    bool SendPDO1(BYTE_t data[8]);
+    bool SendPDO2(BYTE_t data[8]);
+    bool SendPDO1(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);
-    void SendPDO2(BYTE_t m0=0, BYTE_t m1=0, BYTE_t m2=0, BYTE_t m3=0,
+    bool SendPDO2(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);
 
-    void SendSDO(WORD_t idx, BYTE_t subidx, BYTE_t val, bool store=true);
-    void SendSDO(WORD_t idx, BYTE_t subidx, WORD_t val, bool store=true);
-    void SendSDO(WORD_t idx, BYTE_t subidx, LWORD_t val, bool store=true);
+    bool SendSDO(WORD_t idx, BYTE_t subidx, BYTE_t val, bool store=true);
+    bool SendSDO(WORD_t idx, BYTE_t subidx, WORD_t val, bool store=true);
+    bool SendSDO(WORD_t idx, BYTE_t subidx, LWORD_t val, bool store=true);
 
-    void SendSDO(WORD_t idx, BYTE_t val);
-    void SendSDO(WORD_t idx, WORD_t val);
-    void SendSDO(WORD_t idx, LWORD_t val=0);
+    bool SendSDO(WORD_t idx, BYTE_t val);
+    bool SendSDO(WORD_t idx, WORD_t val);
+    bool SendSDO(WORD_t idx, LWORD_t val=0);
 
-    void SendNMT(BYTE_t cmd);
+    bool SendNMT(BYTE_t cmd);
 
-    void RequestSDO(WORD_t idx, BYTE_t subidx=0);
+    bool RequestSDO(WORD_t idx, BYTE_t subidx=0);
 
     void WaitForNextPdo1();

trunk/MagicSoft/Cosy/candrv/vmodican.cc

@@ -232 +232 @@
                 cout << "TxErrCnt=" << (int)msg->data[5] << endl;
             }
-            TerminateApp();
+            //FIXME? TerminateApp();
             return;
         case 0x02:

trunk/MagicSoft/Cosy/cosy.cc

@@ -23 +23 @@
 int main(int argc, char **argv)
 {
+    gLog << "==================================================" << endl;
+    gLog << "                    Cosy V0.1                     " << endl;
+    gLog << "       Magic Drive Control System Softwware       " << endl;
+    gLog << "            Compiled on <" << __DATE__ << ">"       << endl;
+    gLog << "               Using ROOT v" << ROOTVER             << endl;
+    gLog << "==================================================" << endl;
+    gLog << endl;
+
     Timer time;
     time.Now();

trunk/MagicSoft/Cosy/devdrv/macs.cc

@@ -48 +48 @@
     case 0x100a:
         lout << "- " << GetNodeName() << ": Using Software Version V" << dec << (int)(val>>16) << "." << (int)(val&0xff) << endl;
+        fSoftVersion = val;
+        return;
+
+    case 0x100b:
+        // Do not display, this is used for CheckConnection
+        // lout << "Node ID: " << dec << val << endl;
         return;
 
@@ -221 +227 @@
 }
 
-void Macs::InitDevice(Network *net)
-{
-    lout << "- " << GetNodeName() << ": MAC Init device." << endl;
-    NodeDrv::InitDevice(net);
-    lout << "- " << GetNodeName() << ": MAC Init device...done." << endl;
-
-//    SendSDO(0x4003, (LWORD_t)('E'<<24 | 'X'<<16 | 'I'<<8 'T'));
-//    WaitForSdo(0x4003, 0);
-
-/*
-    lout << "- Requesting SDO 0x2002 (vel) of " << (int)GetId() << endl;
-    RequestSDO(0x2002);
-    WaitForSdo(0x2002);
-
-    lout << "- Requesting SDO 0x2003 of " << (int)GetId() << endl;
-    RequestSDO(0x2003);
-    WaitForSdo(0x2003);
-
-    lout << "- Requesting SDO 0x2004 of " << (int)GetId() << endl;
-    RequestSDO(0x2004);
-    WaitForSdo(0x2004);
-    */
+void Macs::CheckConnection()
+{
+    RequestSDO(0x100b);
+    WaitForSdo(0x100b);
+
+    // FIXME! Not statically linked!
+    //    if (fSoftVersion<0x00000035)
+    //        fIsZombie = true;
+}
+
+
+void Macs::Init()
+{
     lout << "- " << GetNodeName() << ": Requesting Mac Software Version." << endl;
     RequestSDO(0x100a);
     WaitForSdo(0x100a);
 
-    if (IsZombie())
-        return;
+    if (fIsZombie)
+    {
+        lout << GetNodeName() << " - InitDevice failed!" << endl;
+        return;
+    }
 
     EnableTimeout(kFALSE);
@@ -259 +259 @@
     SendSDO(0x3000, string('o', 'n'));
     WaitForSdo(0x3000);
-
 
 //    SetHome(250000);
@@ -413 +412 @@
     // or by a positioning command (POSA, ...)
     //
-    lout << "- " << GetNodeName() << ": Starting Posistion Sync Mode." << endl;
+    lout << "- " << GetNodeName() << ": Starting Position Sync Mode." << endl;
     SendSDO(0x3007, 1, string('s', 'y', 'n', 'c'));
     WaitForSdo(0x3007, 1);
@@ -547 +546 @@
 }
 
+// FIXME? Handling of fIsZombie?
 void Macs::HandleError()
 {

trunk/MagicSoft/Cosy/devdrv/macs.h

@@ -11 +11 @@
 private:
     BYTE_t   fMacId;
+
+    LWORD_t  fSoftVersion;
 
     LWORD_t  fVelRes;
@@ -41 +43 @@
     Bool_t HandleTimer(TTimer *t);
 
+    void Init();
+    //bool Reboot();
+
+    //bool InitDevice(Network *);
+
+    //void StartDevice();
+    void StopDevice();
+
+    void HandleSDO(WORD_t idx, BYTE_t subidx, LWORD_t val, timeval_t *tv);
+    void HandleSDOOK(WORD_t idx, BYTE_t subidx);
+    void HandleSDOError(LWORD_t data)           { NodeDrv::HandleSDOError(data); }
+
+    void HandlePDO1(BYTE_t *data, timeval_t *tv);
+    void HandlePDO2(BYTE_t *data, timeval_t *tv);
+
+    void CheckConnection();
+
 public:
+    enum
+    {
+        kNoSync = BIT(0),
+        kPosSync = BIT(1),
+        kVelSync = BIT(2)
+    };
     enum
     {
@@ -54 +79 @@
     Macs(const BYTE_t nodeid, const char *name=NULL, MLog &out=gLog);
     virtual ~Macs();
-
-    void InitDevice(Network *);
-
-    //void StartDevice();
-    void StopDevice();
-
-    void HandleSDO(WORD_t idx, BYTE_t subidx, LWORD_t val, timeval_t *tv);
-    void HandleSDOOK(WORD_t idx, BYTE_t subidx);
-    void HandleSDOError(LWORD_t data)           { NodeDrv::HandleSDOError(data); }
-
-    void HandlePDO1(BYTE_t *data, timeval_t *tv);
-    void HandlePDO2(BYTE_t *data, timeval_t *tv);
 
     void SendMsg(BYTE_t data[6]);

trunk/MagicSoft/Cosy/devdrv/shaftencoder.cc

@@ -51 +51 @@
         }
     case 0x100b:
-        lout << "Node ID: " << dec << val << endl;
+        // Do not display, this is used for CheckConnection
+        // lout << "Node ID: " << dec << val << endl;
         return;
 
@@ -94 +95 @@
 void ShaftEncoder::DisplayVal()
 {
+    if (fIsZombie)
+    {
+        fLabel[0]->SetText(new TGString(""));
+        fLabel[1]->SetText(new TGString(""));
+        fLabel[2]->SetText(new TGString(""));
+        fUpdPos = ~fPos;
+        fUpdVel = ~fVel;
+        fUpdAcc = ~fAcc;
+        return;
+    }
+
     char text[21];
 
@@ -190 +202 @@
 }
 
-void ShaftEncoder::InitDevice(Network *net)
-{
-    NodeDrv::InitDevice(net);
-
+void ShaftEncoder::Init()
+{
     //-----------------------------------------------------------------------
     //                    Start Setup of the Shaft Encoder
@@ -203 +213 @@
     lout << "- " << GetNodeName() << ": Requesting Hardware Type (0x1000)." << endl;
     RequestSDO(0x1000);
-    if (!WaitForSdo(0x1000))
-    {
-        lout << "  ... failed." << endl;
-        fIsZombie = true;
+    WaitForSdo(0x1000);
+
+    if (fIsZombie)
+    {
+        lout << GetNodeName() << " - Init failed!" << endl;
         return;
     }
@@ -265 +276 @@
 }
 
+void ShaftEncoder::CheckConnection()
+{
+    // Request Node number
+    RequestSDO(0x100b);
+    WaitForSdo(0x100b);
+}
+/*
+bool ShaftEncoder::InitDevice(Network *net)
+{
+    NodeDrv::InitDevice(net);
+
+    Init();
+
+    return !fIsZombie;
+}
+
+// --------------------------------------------------------------------------
+//
+// This should be called from a master or main thread to get a node out
+// of the Zombie-Status. 
+//
+bool ShaftEncoder::Reboot()
+{
+    fIsZombie = false;
+
+    Init();
+
+    return !fIsZombie;
+}
+*/
+
 void ShaftEncoder::ReqPos()
 {
@@ -277 +319 @@
 void ShaftEncoder::SetPreset(LWORD_t pre)
 {
+    lout << "- " << GetNodeName() << ": Setting Preset." << endl;
+
+    SendSDO(0x6003, (LWORD_t)fPos);
+    if (!WaitForSdo(0x6003))
+        return;
+
     fPos  = pre%16384;
     fTurn = pre/16384;
-
-    lout << "- " << GetNodeName() << ": Setting Preset." << endl;
-    SendSDO(0x6003, (LWORD_t)fPos);
-    WaitForSdo(0x6003);
 }
 

trunk/MagicSoft/Cosy/devdrv/shaftencoder.h

@@ -32 +32 @@
     void ReqPos();
 
+    void Init();
+    void CheckConnection();
+
 public:
     ShaftEncoder(const BYTE_t nodeid, const char *name=NULL, MLog &out=gLog);
     virtual ~ShaftEncoder();
 
-    void InitDevice(Network *);
+    //bool InitDevice(Network *);
+    //bool Reboot();
 
     //void StartDevice();
@@ -50 +54 @@
     void HandlePDO2(BYTE_t *data, timeval_t *tv) { HandlePDOType2(data, tv); }
 
-    LWORDS_t GetPos() { return fPos+fTurn*fTicks; }
+    LWORDS_t GetPos() { return fIsZombie ? 0 : fPos+fTurn*fTicks; } // FIXME? 0?
     LWORD_t  GetPhysRes() { return fTicks; }
 

trunk/MagicSoft/Cosy/main/MCosy.cc

@@ -41 +41 @@
 //const XY kGearRatio2(GEAR_RATIO_ALT*RES_RE/360.0,  GEAR_RATIO_AZ*RES_RE/360.0);    //[re/deg]
 
+/* +===================================+
+    FIXME: What if fMac3 (Sync) died?
+   +===================================+
+*/
+
 double MCosy::Rad2SE(double rad) const
 {
@@ -150 +155 @@
     // Get the values
     //
-    const int p0 = !fZd1->IsZombieNode() ? fZd1->GetPos() : 0;
-    const int p1 = !fZd2->IsZombieNode() ? fZd2->GetPos() : 0;
-    const int p2 = !fAz->IsZombieNode()  ? fAz->GetPos()  : 0;
-
-    const int a0 = p0; //p0>8192?p0-16384:p0;
-    const int a1 = p1; //p1>8192?p1-16384:p1;
-    const int a2 = p2; //p2>8192?p2-16384:p2;
+    const int p0 = fZd1->GetPos();
+    const int p1 = fZd2->GetPos();
+    const int p2 = fAz->GetPos();
 
     //
     // interpolate shaft encoder positions
     //
-    const float a = (float)(a0-a1)/2;
+    const float p = (float)(p0-p1)/2;
 
     //
     // calculate 'regelabweichung'
     //
-    return ZdAz(a, a2);
+    return ZdAz(p, p2);
 }
 
@@ -177 +178 @@
 Bool_t MCosy::RequestRePos()
 {
-    if (fMac1->IsZombieNode() || fMac2->IsZombieNode())
-        return kTRUE;
-
     //
     // Send request
@@ -189 +187 @@
     // Wait until the objects are received.
     //
+    // FIXME, what when waiting times out (Zombie)
     WaitForSdos();
 
@@ -194 +193 @@
     // If waiting was not interrupted everything is ok. return.
     //
-    if (!StopWaitingForSDO())
+    if (!StopWaitingForSDO() && !HasZombie())
         return kTRUE;
 
@@ -216 +215 @@
 ZdAz MCosy::GetRePos()
 {
-    return !fMac1->IsZombieNode() && !fMac2->IsZombieNode() ? ZdAz(fMac2->GetPos(), fMac1->GetPos()) : ZdAz(0,0);
+    return ZdAz(fMac2->GetPos(), fMac1->GetPos());
 }
 
@@ -304 +303 @@
 int MCosy::StopWaitingForSDO() const
 {
-    return Break() || HasError() || HasZombie();
+    return Break() || HasError();
 }
 
@@ -316 +315 @@
 void MCosy::WaitForEndMovement()
 {
+    // FIXME, what when waiting times out (Zombie)
     WaitForSdos();
 
-    while ((fMac1->IsPositioning() || fMac2->IsPositioning()) && !StopWaitingForSDO())
+    while ((fMac1->IsPositioning() || fMac2->IsPositioning()) && !StopWaitingForSDO() && !HasZombie())
         usleep(1);
 }
@@ -378 +378 @@
     // FIXME: Correct by fOffset ?
 
+    /*
     if (fMac1->IsZombieNode() || fMac2->IsZombieNode())
     {
@@ -383 +384 @@
         return TRUE;
     }
+    */
+
+    //
+    // Make sure that the motors are in sync mode (necessary if the
+    // MACS has been rebooted from a Zombie state.
+    //
+    InitSync();
+    if (fMac3->IsZombieNode())
+        return false;
 
     //
     // Calculate new target position (shortest distance to go)
     //
-    const ZdAz src = GetSePos();//*TMath::Pi()*2/16384;;
+    const ZdAz src = GetSePos();
 
     //
@@ -404 +414 @@
     cout << "Shortest Dest Zd: " << dest.Zd() << "se  Az:" << dest.Az() << "se" << endl;
 
-    for (int i=0; i<10 && !StopWaitingForSDO(); i++)
+    for (int i=0; i<10 && !StopWaitingForSDO() && !HasZombie(); i++)
     {
         //
@@ -450 +460 @@
 
         rd.Round();
+
+        // FIXME? Check for Error or Zombie?
 
         /*
@@ -477 +489 @@
 // in case of success, kFALSE in case of failure.
 //
-Bool_t MCosy::SetVelocity(ZdAz v)
+Bool_t MCosy::SetVelocity(const ZdAz &v)
 {
     //
@@ -488 +500 @@
     // Wait for the objects to be OKed.
     //
+    // FIXME, what when waiting times out (Zombie)
     WaitForSdos();
 
@@ -493 +506 @@
     // If the waiting for the objects wasn't interrupted return kTRUE
     //
-    if (!StopWaitingForSDO())
+    if (!StopWaitingForSDO() && !HasZombie())
         return kTRUE;
 
@@ -513 +526 @@
 // revolution mode.
 //
-void MCosy::InitTracking()
-{
+bool MCosy::InitTracking()
+{
+    // FIXME? Handling of Zombie OK?
     if (fMac1->IsZombieNode() || fMac2->IsZombieNode())
-        return;
+        return false;
 
     //
@@ -523 +537 @@
     fMac2->SetAcceleration(0.90*fMac2->GetVelRes());
     fMac2->SetDeceleration(0.90*fMac2->GetVelRes());
+    if (fMac2->IsZombieNode())
+        return false;
 
     fMac1->SetAcceleration(0.90*fMac1->GetVelRes());
     fMac1->SetDeceleration(0.90*fMac1->GetVelRes());
+    if (fMac1->IsZombieNode())
+        return false;
 
     SetStatus(MCosy::kMoving | MCosy::kTracking);
 
     fMac2->SetRpmMode(TRUE);
+    if (fMac2->IsZombieNode())
+        return false;
+
     fMac1->SetRpmMode(TRUE);
+    if (fMac1->IsZombieNode())
+        return false;
+
+    return true;
 }
 
@@ -640 +665 @@
     // Init accelerations and Rpm Mode
     //
-    InitTracking();
+    if (!InitTracking())
+        return;
 
     XY xy(Rad2Deg(dst.Ra())*24/360, Rad2Deg(dst.Dec()));
@@ -655 +681 @@
     //
     fRaDec = dst;
-
-    if (fMac1->IsZombieNode() || fMac2->IsZombieNode())
-        return;
-
     fTracking = kTRUE;
 
@@ -671 +693 @@
     //
     const float dt = 1;  // 1 second
-    while (!StopWaitingForSDO())
+    while (!StopWaitingForSDO() && !HasZombie())
     {
         //
@@ -774 +796 @@
 void MCosy::StopMovement()
 {
-    if (fMac1->IsZombieNode() || fMac2->IsZombieNode())
-        return;
-
     //
     // Set status to Stopping
@@ -785 +804 @@
     // set deceleration to 50%
     //
-    cout << "Stopping  positioning..." << endl;
+    cout << "Stopping..." << endl;
     fMac1->SetDeceleration(0.5*fMac1->GetVelRes());
+    if (!fMac1->IsZombieNode())
+        fMac1->SetRpmMode(FALSE);
+
     fMac2->SetDeceleration(0.5*fMac2->GetVelRes());
-
-    //
-    // Stop revolution mode (movement)
-    //
-    cout << "Stopping possibleRPM mode..." << endl;
-    fMac1->SetRpmMode(FALSE);
-    fMac2->SetRpmMode(FALSE);
+    if (!fMac2->IsZombieNode())
+        fMac2->SetRpmMode(FALSE);
 
     //
@@ -809 +826 @@
 }
 
-void *MCosy::Proc(int msg, void *mp)
-{
-    lout << "Checking for Zombies" << endl;
+bool MCosy::CheckNetwork()
+{
+    CheckConnections();
     if (HasZombie())
     {
-        lout << "Found Zombies" << endl;
+        lout << "- Found Zombies in Network..." << endl;
         if (!RebootZombies())
-            return (void*)0xebb0;
+            return false;
     }
-
+    return true;
+}
+
+void *MCosy::Proc(int msg, void *mp)
+{
     switch (msg)
     {
@@ -827 +848 @@
     case WM_STOP:
         cout << "MCosy::Proc: Stop." << endl;
+        if (!CheckNetwork())
+            return (void*)0xebb0;
         StopMovement();
         return NULL;
@@ -832 +855 @@
     case WM_PRESET:
         cout << "WM_Preset: start." << endl;
-        if (!fZd1->IsZombieNode()) fZd1->SetPreset();
-        if (!fZd2->IsZombieNode()) fZd2->SetPreset();
-        if (!fAz->IsZombieNode())  fAz->SetPreset();
+        if (!CheckNetwork())
+            return (void*)0xebb0;
+        fZd1->SetPreset();
+        fZd2->SetPreset();
+        fAz->SetPreset();
         cout << "WM_Preset: done. (return 0xaffe)" << endl;
         return (void*)0xaffe;
@@ -841 +866 @@
         {
             cout << "WM_Calib: start." << endl;
+            if (!CheckNetwork())
+                return (void*)0xebb0;
+
             SlaStars sla;
             sla.SetMjd2Now();
@@ -858 +886 @@
             cout << "Got  Zd: " << sepos.Zd() << " Az: " << sepos.Az() << endl;
 
-            if (!fZd1->IsZombieNode())
-                fZd1->SetPreset(za.Zd());
-            if (!fZd2->IsZombieNode())
-                fZd2->SetPreset(-za.Zd());
-            if (!fAz->IsZombieNode())
-                fAz->SetPreset(za.Az());
+            fZd1->SetPreset(za.Zd());
+            fZd2->SetPreset(-za.Zd());
+            fAz->SetPreset(za.Az());
 
             cout << "WM_Calib: done. (return 0xaffe)" << endl;
@@ -897 +922 @@
         cout << "WM_Position: start." << endl;
         {
+            if (!CheckNetwork())
+                return (void*)0xebb0;
+
             ZdAz dest = *((ZdAz*)mp);
-
             SetPosition(dest*kDeg2Rad);
         }
@@ -907 +934 @@
         cout << "WM_Track: START" << endl;
         {
+            if (!CheckNetwork())
+                return (void*)0xebb0;
+
             RaDec dest = *((RaDec*)mp);
             TrackPosition(dest*kDeg2Rad);
@@ -933 +963 @@
     case WM_HOME:
         cout << "WM_Home: START" << endl;
-        if (!fMac1->IsZombieNode() && !fMac2->IsZombieNode())
+        if (!CheckNetwork())
+            return (void*)0xebb0;
+        else
         {
             cout << "Going Home..." << endl;
@@ -976 +1008 @@
     case WM_QUIT:
         cout << "WM_Quit: now." << endl;
+        if (!CheckNetwork())
+        {
+            lout << "ERROR: Cannot shutdown CANbus network." << endl;
+            return (void*)0xebb0;
+        }
         TerminateApp();
         cout << "WM_Quit: done." << endl;
@@ -1004 +1041 @@
         resreaz = fMac1->GetRes();
     else
-        if (!fMac3->IsZombieNode())
+        if (fMac3 && !fMac3->IsZombieNode())
             resreaz = fMac3->GetRes();
         else
@@ -1040 +1077 @@
 }
 
+void MCosy::InitSync()
+{
+    if (!fMac3)
+        return;
+
+    const int res = fMac3->GetVelRes();
+
+    fMac3->SetVelocity(res);
+    fMac3->SetAcceleration(res);
+    fMac3->SetDeceleration(res);
+    fMac3->StartPosSync();
+}
+
 void MCosy::TalkThread()
 {
-    if (fMac1->IsZombieNode() || fMac2->IsZombieNode())
+    /* ========== FIXME? =============
+     if (fMac1->IsZombieNode() || fMac2->IsZombieNode())
         return;
-
+     */
     fMac1->ReqPos();
     fMac2->ReqPos();
-
-    if (fMac3)
-    {
-        const int res = fMac3->GetVelRes();
-
-        fMac3->SetVelocity(res);
-        fMac3->SetAcceleration(res);
-        fMac3->SetDeceleration(res);
-        fMac3->StartPosSync();
-    }
 
     if (fZd1->IsZombieNode() || fZd2->IsZombieNode() || fAz->IsZombieNode())
@@ -1227 +1268 @@
     // Update Gui, foremer MTGui.
     //
-    if (!fZd1->IsZombieNode()) fZd1->DisplayVal();
-    if (!fZd2->IsZombieNode()) fZd2->DisplayVal();
-    if (!fAz->IsZombieNode())  fAz->DisplayVal();
+    fZd1->DisplayVal();
+    fZd2->DisplayVal();
+    fAz->DisplayVal();
 
     ZdAz seist = GetSePos()*2*TMath::Pi()/16384; // [se]
@@ -1238 +1279 @@
     avail |= !fMac1->IsZombieNode() ? 0x01 : 0;
     avail |= !fMac2->IsZombieNode() ? 0x02 : 0;
-    avail |= !fMac3->IsZombieNode() ? 0x04 : 0;
+    avail |= (fMac3 && !fMac3->IsZombieNode()) ? 0x04 : 0;
     avail |= !fZd1->IsZombieNode()  ? 0x08 : 0;
     avail |= !fZd2->IsZombieNode()  ? 0x10 : 0;
@@ -1268 +1309 @@
     // Don't call this function twice!
     Network::Start();
-/*
-    if (fMac1)
-        if (fMac1->IsZombieNode()) { delete fMac1; fMac1=NULL; }
-    if (fMac2)
-        if (fMac2->IsZombieNode()) { delete fMac2; fMac2=NULL; }
-    if (fMac3)
-        if (fMac3->IsZombieNode()) { delete fMac3; fMac3=NULL; }
-*/
-/*    if (fZd1)
-        if (fZd1->IsZombieNode())  { delete fZd1;  fZd1=NULL; }
-        else */fZd1->SetDisplay(fWin->GetLabel2());
-
-/*    if (fZd2)
-        if (fZd2->IsZombieNode())  { delete fZd2;  fZd2=NULL; }
-        else */fZd2->SetDisplay(fWin->GetLabel3());
-
-/*    if (fAz)
-        if (fAz->IsZombieNode())   { delete fAz;   fAz=NULL; }
-        else */fAz->SetDisplay(fWin->GetLabel1());
 
     ReadConfig();
@@ -1292 +1314 @@
     lout << "- Starting TX Thread." << endl;
     fTTalk = new MTTalk(this);
-    //    fTGui = new MTGui(this);
 
     lout << "- Starting GUI update." << endl;
@@ -1361 +1382 @@
     lout << "- Starting GUI." << endl;
     fWin=new MGCosy(this, gClient->GetRoot(), 1, 1);
-
-    fAz->SetDisplay(fWin->GetLabel1());
-    fZd1->SetDisplay(fWin->GetLabel2());
-    fZd2->SetDisplay(fWin->GetLabel3());
-
-    lout.SetOutputGui(fWin->GetLog(), kTRUE);
 }
 
@@ -1397 +1412 @@
     lout << "- Starting GUI." << endl;
     fWin=new MGCosy(this, gClient->GetRoot(), 1, 1);
-
-    fAz->SetDisplay(fWin->GetLabel1());
-    fZd1->SetDisplay(fWin->GetLabel2());
-    fZd2->SetDisplay(fWin->GetLabel3());
-
-    lout.SetOutputGui(fWin->GetLog(), kTRUE);
 }
 
@@ -1420 +1429 @@
     lout << "- Starting GUI." << endl;
     fWin=new MGCosy(this, gClient->GetRoot(), 1, 1);
-
-    lout.SetOutputGui(fWin->GetLog(), kTRUE);
 }
 
@@ -1451 +1458 @@
     }
 
+    lout.SetOutputGui(fWin->GetLog(), kTRUE);
+
+    fZd1->SetDisplay(fWin->GetLabel2());
+    fZd2->SetDisplay(fWin->GetLabel3());
+    fAz->SetDisplay(fWin->GetLabel1());
+
     int i=0;
     char name[100];
@@ -1503 +1516 @@
     cout << "Deleting Nodes." << endl;
 
-    if (fAz)   delete fAz;
-    if (fZd1)  delete fZd1;
-    if (fZd2)  delete fZd2;
-    if (fMac1) delete fMac1;
-    if (fMac2) delete fMac2;
-    if (fMac3) delete fMac3;
+    delete fAz;
+    delete fZd1;
+    delete fZd2;
+    delete fMac1;
+    delete fMac2;
+    if (fMac3)
+        delete fMac3;
 
     cout << "Deleting MGCosy." << endl;

trunk/MagicSoft/Cosy/main/MCosy.h

@@ -107 +107 @@
 
     Bool_t RequestRePos();
-    Bool_t SetVelocity(ZdAz v);
+    Bool_t SetVelocity(const ZdAz &v);
     void SetPosVelocity(const Float_t ratio, Float_t vel, Float_t acc);
 
     void DoRelPos(const ZdAz &rd, const Bool_t axe1, const Bool_t axe2);
 
-    void InitTracking();
+    void InitSync();
+    bool InitTracking();
     void LimitSpeed(ZdAz *vt, const ZdAz &vcalc) const;
 
@@ -135 +136 @@
     void ReadConfig();
 
+    bool CheckNetwork();
+
 public:
     MCosy(int mode, const char *dev, const int baud, MLog &out=gLog);