Ignore:
Timestamp:
01/23/03 13:32:58 (22 years ago)
Author:
tbretz
Message:
*** empty log message ***
Location:
trunk/MagicSoft/Cosy/candrv
Files:
8 edited

Legend:

Unmodified
Added
Removed

  • trunk/MagicSoft/Cosy/candrv/canopen.cc

    r1275 r1727  
    1616!
    1717!
    18 !   Author(s): Thomas Bretz <mailto:tbretz@uni-sw.gwdg.de>, 2001
    19 !
    20 !   Copyright: MAGIC Software Development, 2000-2001
     18!   Author(s): Thomas Bretz <mailto:tbretz@astro.uni-wuerzburg.de>, 2003
     19!
     20!   Copyright: MAGIC Software Development, 2000-2003
    2121!
    2222!
    … …  
    8484{
    8585    const WORD_t fcode = cobid >> 7;
     86    const BYTE_t  node = cobid & 0x1f;
    8687
    8788    switch (fcode)
    … …  
    9495        return;
    9596
    96     case kSYNC:
    97         cout << "Sync" << endl;
     97    case kEMERGENCY: // also case kSYNC:
     98        if (cobid==0)
     99            cout << "Sync" << endl;
     100        else
     101        {
     102            cout << "EMERGENCY Node #" << dec << (int)data[1] << endl;
     103            HandleEmergency(node, tv);
     104        }
     105        return;
     106
     107    case kNodeguard:
     108        //cout << "Nodeguard Node #" << dec << (int)node << endl;
     109        HandleNodeguard(node, tv);
    98110        return;
    99111
    100112    case kSDO_RX:
    101113        {
    102             const BYTE_t  node   = cobid & 0x1f;
    103114            const BYTE_t  cmd    = data[0];
    104115            const LWORD_t dat    = data[4] | (data[5]<<8) | (data[6]<<16) | (data[7]<<24);
    … …  
    114125    case kPDO1_TX:
    115126        {
    116             const BYTE_t node = cobid & 0x1f;
    117127            HandlePDO1(node, data, tv);
    118128            pthread_cond_broadcast(&fPdoCond[node-1][0]);
    … …  
    122132    case kPDO2_TX:
    123133        {
    124             const BYTE_t node = cobid & 0x1f;
    125134            HandlePDO2(node, data, tv);
    126135            pthread_cond_broadcast(&fPdoCond[node-1][1]);
    … …  
    130139    case kPDO3_TX:
    131140        {
    132             const BYTE_t node = cobid & 0x1f;
    133141            HandlePDO3(node, data, tv);
    134142            pthread_cond_broadcast(&fPdoCond[node-1][2]);
    … …  
    138146    case kPDO4_TX:
    139147        {
    140             const BYTE_t node = cobid & 0x1f;
    141148            HandlePDO4(node, data, tv);
    142149            pthread_cond_broadcast(&fPdoCond[node-1][3]);
    … …  
    145152    }
    146153
    147     const BYTE_t node = cobid & 0x1f;
    148     cout << "Function Code: 0x" << hex << fcode << "  Node: " << dec << (int)node << endl;
     154    cout << "CanOpen::HandleCanMessage - Unhandled Message: Function Code=0x" << hex << fcode << "  Node #" << dec << (int)node << endl;
    149155}
    150156
    … …  
    168174{
    169175    EnableCanMsg(node, kEMERGENCY);
     176}
     177
     178// --------------------------------------------------------------------------
     179//
     180// Enables Nodeguard messages for a given node
     181//
     182void CanOpen::EnableNodeguard(BYTE_t node)
     183{
     184    EnableCanMsg(node, kNodeguard);
    170185}
    171186
    … …  
    372387// --------------------------------------------------------------------------
    373388//
    374 // Send an NMT Message to the given node with command cmd
     389// Send a NMT Message to the given node with command cmd
    375390//
    376391void CanOpen::SendNMT(BYTE_t node, BYTE_t cmd)
    … …  
    381396// --------------------------------------------------------------------------
    382397//
     398// Send a Nodeguard Message to the given node with command cmd
     399//
     400void CanOpen::SendNodeguard(BYTE_t node)
     401{
     402    BYTE_t msg[8] = { 0, 0, 0, 0, 0, 0, 0, 0 };
     403    SendCanFrame(CobId(node, kNodeguard), msg, 1);
     404}
     405
     406// --------------------------------------------------------------------------
     407//
    383408// Decodes node and function code into a CobId
    384409//

  • trunk/MagicSoft/Cosy/candrv/network.cc

    r1703 r1727  
    8989void Network::HandleSDO(BYTE_t node, BYTE_t cmd, WORD_t idx, BYTE_t subidx, LWORD_t data, timeval_t *tv)
    9090{
    91     switch (cmd)
    92     {
    93     case kSDO_TX4:       // answer to 0x40 with 4 bytes of data
    94         if (fNodes[node])
     91    if (fNodes[node])
     92        switch (cmd)
    9593        {
     94        case kSDO_TX4:       // answer to 0x40 with 4 bytes of data
    9695            fNodes[node]->HandleSDO(idx, subidx, data, tv);
    9796            return;
     97
     98        case kSDO_TX3:       // answer to 0x40 with 2 bytes of data
     99            fNodes[node]->HandleSDO(idx, subidx, data>>16, tv);
     100            return;
     101
     102        case kSDO_TX1:       // answer to 0x40 with 1 byte  of data
     103            fNodes[node]->HandleSDO(idx, subidx, data>>24, tv);
     104            return;
     105
     106        case kSDO_TX_OK:     // answer to a SDO_TX message
     107            fNodes[node]->HandleSDOOK(idx, subidx, tv);
     108            return;
     109
     110        case kSDO_TX_ERROR:  // error message (instead of 0x60)
     111            fNodes[node]->HandleSDOError(data);
     112            return;
    98113        }
    99         break;
    100 
    101     case kSDO_TX3:       // answer to 0x40 with 2 bytes of data
    102         if (fNodes[node])
    103         {
    104             fNodes[node]->HandleSDO(idx, subidx, data>>16, tv);
    105             return;
    106         }
    107         break;
    108 
    109     case kSDO_TX1:       // answer to 0x40 with 1 byte  of data
    110         if (fNodes[node])
    111         {
    112             fNodes[node]->HandleSDO(idx, subidx, data>>24, tv);
    113             return;
    114         }
    115         break;
    116 
    117     case kSDO_TX_OK:     // answer to a SDO_TX message
    118         if (fNodes[node])
    119         {
    120             fNodes[node]->HandleSDOOK(idx, subidx);
    121             return;
    122         }
    123         break;
    124 
    125     case kSDO_TX_ERROR:  // error message (instead of 0x60)
    126         if (fNodes[node])
    127         {
    128             fNodes[node]->HandleSDOError(data);
    129             return;
    130         }
    131         break;
    132     }
     114
    133115    cout << dec << setfill('0');
    134     cout << "Node=" << (int)node  << " Cmd=0x" << hex << (int)cmd << ": ";
     116    cout << "Network::HandleSDO: Node=" << (int)node  << " Cmd=0x" << hex << (int)cmd << ": ";
    135117    cout << "Sdo=" << idx  << "/" << (int)subidx << ": 0x" << setw(8) << data;
    136118    cout << endl;
    … …  
    145127    if (!fNodes[node])
    146128    {
    147         cout << "Node " << dec << (int)node << ", PDO1: " << hex;
     129        cout << "Network::HandlePDO1: Node #" << dec << (int)node << " not found - PDO1: " << hex;
    148130        for (int i=0; i<8; i++)
    149131            cout << " 0x" << (int)data[i];
    … …  
    163145    if (!fNodes[node])
    164146    {
    165         cout << "Node " << dec << (int)node << ", PDO2: " << hex;
     147        cout << "Network::HandlePDO2: Node #" << dec << (int)node << " not found - PDO2: " << hex;
    166148        for (int i=0; i<8; i++)
    167149            cout << " 0x" << (int)data[i];
    … …  
    181163    if (!fNodes[node])
    182164    {
    183         cout << "Node " << dec << (int)node << ", PDO3: " << hex;
     165        cout << "Network::HandlePDO3: Node #" << dec << (int)node << " not found - PDO3: " << hex;
    184166        for (int i=0; i<8; i++)
    185167            cout << " 0x" << (int)data[i];
    … …  
    199181    if (!fNodes[node])
    200182    {
    201         cout << "Node " << dec << (int)node << ", PDO4: " << hex;
     183        cout << "Network::HandlePDO4: Node #" << dec << (int)node << " not found - PDO4: " << hex;
    202184        for (int i=0; i<8; i++)
    203185            cout << " 0x" << (int)data[i];
    … …  
    208190    fNodes[node]->HandlePDO4(data, tv);
    209191}
     192
     193// --------------------------------------------------------------------------
     194//
     195// Distributes Nodeguard messages to the correspoding node calling
     196// HandleNodeguard
     197//
     198void Network::HandleNodeguard(BYTE_t node, timeval_t *tv)
     199{
     200    if (!fNodes[node])
     201    {
     202        cout << "Network::HandleNodeguard: Node #" << dec << (int)node << " not found: Nodeguard." << endl;
     203        return;
     204    }
     205
     206    fNodes[node]->HandleNodeguard(tv);
     207}
     208
     209// --------------------------------------------------------------------------
     210//
     211// Distributes Emergency messages to the correspoding node calling
     212// HandleEmergency
     213//
     214void Network::HandleEmergency(BYTE_t node, timeval_t *tv)
     215{
     216    if (!fNodes[node])
     217    {
     218        cout << "Network::HandleEmergency: Node #" << dec << (int)node << " not found: Emergency." << endl;
     219        return;
     220    }
     221
     222    fNodes[node]->HandleEmergency(tv);
     223}
     224
    210225
    211226// --------------------------------------------------------------------------
    … …  
    281296            continue;
    282297
    283         lout << "- Node #" << dec << i << " '" << fNodes[i]->GetNodeName();
    284         lout << "' has error #" << fNodes[i]->GetError() << endl;
     298        //lout << "- Node #" << dec << i << " '" << fNodes[i]->GetNodeName();
     299        //lout << "' has error #" << fNodes[i]->GetError() << endl;
    285300    }
    286301

  • trunk/MagicSoft/Cosy/candrv/network.h

    r1703 r1727  
    1616    void HandlePDO3(BYTE_t node, BYTE_t *data, timeval_t *tv);
    1717    void HandlePDO4(BYTE_t node, BYTE_t *data, timeval_t *tv);
     18    void HandleNodeguard(BYTE_t node, timeval_t *tv);
     19    void HandleEmergency(BYTE_t node, timeval_t *tv);
    1820
    1921    void InitNodes();

  • trunk/MagicSoft/Cosy/candrv/nodedrv.cc

    r1703 r1727  
    3333//  virtual void StopDevice()
    3434//  virtual void HandleSDO(WORD_t idx, BYTE_t subidx, LWORD_t val, timeval_t *tv)
    35 //  virtual void HandleSDOOK(WORD_t idx, BYTE_t subidx)
     35//  virtual void HandleSDOOK(WORD_t idx, BYTE_t subidx, timeval_t *tv)
    3636//  virtual void HandleSDOError(LWORD_t data)
    3737//  virtual void HandlePDO1(BYTE_t *data, timeval_t *tv)
    … …  
    4848#include <iostream.h>
    4949
     50#include <TTimer.h>
     51
     52#include "base/timer.h"
    5053#include "network.h"
    5154#include "MLogManip.h"
    … …  
    7679    }
    7780
     81    fTimerOn = kFALSE;
     82    fTimeout = new TTimer(this, 100, kFALSE); // 100ms, asynchronous
     83
    7884    lout << "- Node #" << (int)nodeid << " (" << name << ") initialized." << endl;
    79 }
    80 
    81 // --------------------------------------------------------------------------
    82 //
    83 // Empty destructor
     85
     86}
     87
     88// --------------------------------------------------------------------------
     89//
     90// destructor
    8491//
    8592NodeDrv::~NodeDrv()
    8693{
     94    fTimerOn = kFALSE;
     95    delete fTimeout;
    8796}
    8897
    … …  
    122131    EnableCanMsg(kSDO_RX);
    123132    EnableCanMsg(kSDO_TX);
     133    EnableCanMsg(kNodeguard);
     134    EnableCanMsg(kEMERGENCY);
    124135
    125136    fIsZombie = kFALSE;
    … …  
    135146// This output is never redirected to the GUI
    136147//
    137 void NodeDrv::HandleSDOOK(WORD_t idx, BYTE_t subidx)
     148void NodeDrv::HandleSDOOK(WORD_t idx, BYTE_t subidx, timeval_t *tv)
    138149{
    139150    const Bool_t gui = lout.IsOutputDeviceEnabled(MLog::eGui);
    … …  
    367378// --------------------------------------------------------------------------
    368379//
    369 // Send an NMT message (command) to this device
     380// Send a NMT message (command) to this device
    370381//
    371382// The message is not send if the node has the status Zombie.
    … …  
    377388        fNetwork->SendNMT(fId, cmd);
    378389    return !fIsZombie;
     390}
     391
     392// --------------------------------------------------------------------------
     393//
     394// Send a Nodeguard message (command) to this device
     395//
     396void NodeDrv::SendNodeguard()
     397{
     398    fNetwork->SendNodeguard(fId);
    379399}
    380400
    … …  
    404424
    405425    if (!rc)
     426    {
     427        lout << "NodeDrv::WaitForSdo: 0x" << hex << idx << "/" << dec << (int)subidx << " " << GetNodeName() << " --> ZOMBIE!" << endl;
     428        SetZombie();
     429    }
     430/*
     431    if (HasError())
     432    {
     433        lout << "NodeDrv::WaitForSdo: HasError 0x" << hex << idx << "/" << dec << (int)subidx << " " << GetNodeName() << " --> ZOMBIE!" << endl;
    406434        fIsZombie = kTRUE;
    407 
     435    }
     436*/
    408437    return fIsZombie ? false : rc;
    409438}
    … …  
    453482}
    454483
     484// --------------------------------------------------------------------------
     485//
     486// Start the standard CANopen guarding of the device.
     487// While ms is the guard time in millisec. This is the time between
     488// two requests for a Nodeguard message.
     489// ltf is the LifeTimeFactor. This means how often it is checked, that at
     490// least one Nodeguard message was answered.
     491//
     492void NodeDrv::StartGuarding()
     493{
     494    if (fTimerOn)
     495        return;
     496
     497    SendNodeguard();
     498
     499    fTimerOn = kTRUE;
     500    fTimeout->SetTime(fGuardTime);
     501    fTimeout->Reset();
     502    fTimeout->TurnOn();
     503    //fTimeout->Start(fGuardTime, kTRUE);
     504
     505    lout << "- " << GetNodeName() << ": Guarding (" << dec;
     506    lout << fLifeTimeFactor << "*" << fGuardTime << "ms) started." << endl;
     507}
     508
     509void NodeDrv::StartGuarding(Int_t ms, Int_t ltf)
     510{
     511    if (fTimerOn)
     512    {
     513        lout << "- " << GetNodeName() << ": ERROR - Guarding already started." << endl;
     514        return;
     515    }
     516    fGuardTime      = ms;
     517    fLifeTimeFactor = ltf;
     518
     519    StartGuarding();
     520}
     521
     522void NodeDrv::StopGuarding()
     523{
     524    fTimeout->TurnOff();
     525    fTimerOn = kFALSE;
     526
     527    lout << "- " << GetNodeName() << ": Guarding stopped." << endl;
     528}
     529
     530// --------------------------------------------------------------------------
     531//
     532// Handle the Nodeguard-Timer Event.
     533// It checks whether the node timed out. If it timed out it is set to
     534// the Zombie state.
     535// A new Nodeguard request is send and a new timer event is triggered.
     536//
     537Bool_t NodeDrv::HandleTimer(TTimer *t)
     538{
     539    //
     540    // WARNING:
     541    //           It seems, that you should never access ANY output from
     542    //           here. Neither the GUI, nor COUT. This can result in
     543    //           'unexpected async reply'
     544    //
     545
     546   /*
     547     Fons:
     548     -----
     549
     550     timers never trigger at the same time or when in a TTimer::Notify.
     551     Little explanation:
     552
     553     - there are two types of timers synchronous and a-synchronous.
     554     - synchronous timers are only handled via the ROOT eventloop
     555       (see TUnixSystem::DispatchOneEvent()). If there are no mouse/keyboard
     556       events then the synchronous timer queue is checked. So if the processing
     557       of a mouse/keyboard event takes a long time synchronous timers are not
     558       called for a while. To prevent this from happening one can call in long
     559       procedures gSystem->ProcessEvents(). The system schedules only the
     560       next timer in the queue when the current one's Notify() has finished.
     561     - a-synchronous timers are triggered via SIGALARM, i.e. the program is
     562       interupted and execution jumps to the Notify() function. When the
     563       notify is finished the next a-sync timer is scheduled and the system
     564       resumes from the place where it was initially interrupted. One of the
     565       things to remember when using a-sync timers is don't make any graphics
     566       calls in them. X11 is not re-entrant and it might be that the SIGALARM
     567       signal interrupted the system while being in X11. A-sync timers are best
     568       used to set flags that you can test at a convenient and controlled
     569       time.
     570       */
     571    if (fIsZombie)
     572        return kTRUE;
     573
     574    Timer time;
     575    Double_t now = time.Now();
     576    if (now > fTimeoutTime)
     577    {
     578        //cout << "ERROR - " << GetNodeName() << " didn't respond in timeout window." << endl;
     579        //lout << "ERROR - " << GetNodeName() << " didn't respond in timeout window." << endl;
     580        //cout << dec << "+" << (int)GetId() << ": Handle: " << fmod(now, 500) << endl;
     581        //cout << dec << "+" << (int)GetId() << ": Handle: " << fmod(fTimeoutTime, 500) << endl;
     582        //cout << fGuardTime << endl;
     583        fIsZombie = true;
     584        //SetZombie();
     585
     586        return kTRUE;
     587    }
     588
     589    SendNodeguard();
     590
     591    return kTRUE;
     592}
     593
     594// --------------------------------------------------------------------------
     595//
     596// Set the timeout timer to the time the event was received plus the
     597// guard time times lifetimefactor.
     598//
     599void NodeDrv::HandleNodeguard(timeval_t *tv)
     600{
     601    Timer t(tv);
     602    fTimeoutTime = t + (fGuardTime*fLifeTimeFactor/1000.);
     603}
     604
     605void NodeDrv::SetZombie()
     606{
     607    fIsZombie = true;
     608    StopGuarding();
     609}

  • trunk/MagicSoft/Cosy/candrv/nodedrv.h

    r1703 r1727  
    2020class Network;
    2121
    22 class NodeDrv : public Log
     22class NodeDrv : public Log, public TObject
    2323{
    2424private:
    … …  
    3030    int fError;
    3131
     32    Bool_t fIsZombie; // A Zombie node is a node which doesn't answer...
     33
     34    TTimer  *fTimeout;
     35    Int_t    fGuardTime;      // Guardtime [ms]
     36    Int_t    fLifeTimeFactor;
     37    Double_t fTimeoutTime;
     38    Bool_t   fTimerOn;
     39
     40    Bool_t HandleTimer(TTimer *t);
     41
    3242protected:
    3343    void SetError(int err) { fError = err; }
    3444    void DelError()        { fError = 0; }
    3545
    36     Bool_t fIsZombie; // A Zombie node is a node which doesn't answer...
     46    Int_t GetLifeTimeFactor() const { return fLifeTimeFactor; }
     47    Int_t GetGuardTime() const      { return fGuardTime; }
     48
     49    virtual void SetZombie();
    3750
    3851public:
    … …  
    5669
    5770    virtual void HandleSDO(WORD_t idx, BYTE_t subidx, LWORD_t val, timeval_t *tv);
    58     virtual void HandleSDOOK(WORD_t idx, BYTE_t subidx);
     71    virtual void HandleSDOOK(WORD_t idx, BYTE_t subidx, timeval_t *tv);
    5972    virtual void HandleSDOError(LWORD_t data);
    6073
    61     virtual void HandlePDO1(BYTE_t *data, timeval_t *tv) {};
    62     virtual void HandlePDO2(BYTE_t *data, timeval_t *tv) {};
    63     virtual void HandlePDO3(BYTE_t *data, timeval_t *tv) {};
    64     virtual void HandlePDO4(BYTE_t *data, timeval_t *tv) {};
     74    virtual void HandlePDO1(BYTE_t *data, timeval_t *tv) {}
     75    virtual void HandlePDO2(BYTE_t *data, timeval_t *tv) {}
     76    virtual void HandlePDO3(BYTE_t *data, timeval_t *tv) {}
     77    virtual void HandlePDO4(BYTE_t *data, timeval_t *tv) {}
     78    virtual void HandleNodeguard(timeval_t *tv);
     79    virtual void HandleEmergency(timeval_t *tv) {}
    6580
    6681    bool SendPDO1(BYTE_t data[8]);
    … …  
    8095
    8196    bool SendNMT(BYTE_t cmd);
     97    virtual void SendNodeguard();
    8298
    8399    bool RequestSDO(WORD_t idx, BYTE_t subidx=0);
    … …  
    93109    void EnableCanMsg(BYTE_t fcode);
    94110
     111    void StartGuarding();
     112    void StartGuarding(Int_t ms, Int_t ltf);
     113    void StopGuarding();
     114
    95115    ClassDef(NodeDrv, 0) // base class for an object describing hardware
    96116};

  • trunk/MagicSoft/Cosy/candrv/vmodican.cc

    r1703 r1727  
    409409    while (!Send(&msg));      /* transmitt to module */
    410410
    411     lout << "- Baudrate set to " << rate << "bps" << endl;
     411    lout << "- Baudrate set to " << rate << "kbps" << endl;
    412412}
    413413
    … …  
    904904//   */
    905905//
    906 void VmodIcan::SendCanFrame(WORD_t cobid, BYTE_t m[8])
    907 {
    908     const WORD_t desc = MsgDescr(cobid, 8);
     906void VmodIcan::SendCanFrame(WORD_t cobid, BYTE_t m[8], BYTE_t rtr)
     907{
     908    const WORD_t desc = MsgDescr(cobid, 8, rtr);
    909909
    910910    Message msg;

  • trunk/MagicSoft/Cosy/candrv/vmodican.h

    r1140 r1727  
    7373    void EnableCobId(WORD_t cobid, int flag=TRUE);
    7474
    75     void SendCanFrame(WORD_t cobid, BYTE_t m[8]);
     75    void SendCanFrame(WORD_t cobid, BYTE_t m[8], BYTE_t rtr=0);
    7676    void SendCanFrame(WORD_t cobid,
    7777                      BYTE_t m0=0, BYTE_t m1=0, BYTE_t m2=0, BYTE_t m3=0,
Note: See TracChangeset for help on using the changeset viewer.