source: trunk/MagicSoft/Cosy/candrv/nodedrv.cc@ 2019

/* ======================================================================== *\
!
! *
! * This file is part of Stesy, the MAGIC Steering System
! * Software. It is distributed to you in the hope that it can be a useful
! * and timesaving tool in analysing Data of imaging Cerenkov telescopes.
! * It is distributed WITHOUT ANY WARRANTY.
! *
! * Permission to use, copy, modify and distribute this software and its
! * documentation for any purpose is hereby granted without fee,
! * provided that the above copyright notice appear in all copies and
! * that both that copyright notice and this permission notice appear
! * in supporting documentation. It is provided "as is" without express
! * or implied warranty.
! *
!
!
!   Author(s): Thomas Bretz <mailto:tbretz@uni-sw.gwdg.de>, 2001
!
!   Copyright: MAGIC Software Development, 2000-2001
!
!
\* ======================================================================== */

///////////////////////////////////////////////////////////////////////
//
// NodeDrv
//
// Base class for a class describing the interface for the CAN nodes.
//
// to be overloaded:
//  virtual void Init()
//  virtual void StopDevice()
//  virtual void HandleSDO(WORD_t idx, BYTE_t subidx, LWORD_t val, timeval_t *tv)
//  virtual void HandleSDOOK(WORD_t idx, BYTE_t subidx, timeval_t *tv)
//  virtual void HandleSDOError(LWORD_t data)
//  virtual void HandlePDO1(BYTE_t *data, timeval_t *tv)
//  virtual void HandlePDO2(BYTE_t *data, timeval_t *tv)
//  virtual void HandlePDO3(BYTE_t *data, timeval_t *tv)
//  virtual void HandlePDO4(BYTE_t *data, timeval_t *tv)
//  virtual bool Reboot();
//  virtual void CheckConnection();
//
///////////////////////////////////////////////////////////////////////
#include "nodedrv.h"

#include <iomanip.h>
#include <iostream.h>

#include <TTimer.h>

#include "base/timer.h"
#include "network.h"
#include "MLogManip.h"

ClassImp(NodeDrv);

// --------------------------------------------------------------------------
//
// Constructor for one node. Sets the Node Id (<32) the logging stream
// 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(kTRUE)
{
    if (nodeid>0x1f)
    {
        cout << "SetNode - Error: Only node Numbers < 32 are allowed"<< endl;
        return;
    }

    fId = nodeid;

    if (name)
        fName = name;
    else
    {
        fName = "Node#";
        fName += (int)nodeid;
    }

    fTimerOn = kFALSE;
    fTimeout = new TTimer(this, 100, kFALSE); // 100ms, asynchronous

    lout << "- Node #" << (int)nodeid << " (" << name << ") initialized." << endl;

}

// --------------------------------------------------------------------------
//
// destructor
//
NodeDrv::~NodeDrv()
{
    fTimerOn = kFALSE;
    delete fTimeout;
}

// --------------------------------------------------------------------------
//
// This should be called from a master or main thread to get a node out
// of the Zombie-Status. Overload it by your needs.
//
bool NodeDrv::Reboot()
{
    fIsZombie = false;

    Init();

    return !fIsZombie;
}

// --------------------------------------------------------------------------
//
// Init device, sets the pointer to the whole network and enables
// the Can messages to be passed through the interface:
//   PDO1 tx
//   PDO2 tx
//   PDO3 tx
//   PDO4 tx
//   SDO rx
//   SDO tx
//
bool NodeDrv::InitDevice(Network *net)
{
    fNetwork = net;

    EnableCanMsg(kPDO1_TX);
    EnableCanMsg(kPDO2_TX);
    EnableCanMsg(kPDO3_TX);
    EnableCanMsg(kPDO4_TX);
    EnableCanMsg(kSDO_RX);
    EnableCanMsg(kSDO_TX);
    EnableCanMsg(kNodeguard);
    EnableCanMsg(kEMERGENCY);

    fIsZombie = kFALSE;

    Init();

    return !fIsZombie;
}

// --------------------------------------------------------------------------
//
// Print an "SDO idx/subidx set." from this device message.
// This output is never redirected to the GUI
//
void NodeDrv::HandleSDOOK(WORD_t idx, BYTE_t subidx, timeval_t *tv)
{
    const Bool_t gui = lout.IsOutputDeviceEnabled(MLog::eGui);

    if (gui)
        lout << ddev(MLog::eGui);

    lout << hex << setfill('0');
    lout << "Node #" << dec << (int)fId << ": Sdo=" << hex << idx  << "/" << (int)subidx << " set.";
    lout << endl;

    if (gui)
        lout << edev(MLog::eGui);
}

// --------------------------------------------------------------------------
//
// Print an error message with the corresponding data from this device.
//
void NodeDrv::HandleSDOError(LWORD_t data)
{
    lout << "Nodedrv: SDO Error: Entry not found in dictionary (data=0x";
    lout << hex << setfill('0') << setw(4) << data << ")";
    lout << endl;
}

// --------------------------------------------------------------------------
//
// Prints the received SDo from this device
//
void NodeDrv::HandleSDO(WORD_t idx, BYTE_t subidx, LWORD_t val, timeval_t *tv)
{
    cout << "SdoRx: Idx=0x"<< hex << idx << "/" << (int)subidx;
    cout << ", val=0x" << val << endl;
}

// --------------------------------------------------------------------------
//
// Sends the given PDO1 through the network to this device
// A PDO is carrying up to eight bytes of information.
//
// 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;
}

// --------------------------------------------------------------------------
//
// Sends the given PDO2 through the network to this device
// A PDO is carrying up to eight bytes of information.
//
// 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;
}

// --------------------------------------------------------------------------
//
// Sends the given PDO1 through the network to this device
// A PDO is carrying up to eight bytes of information.
//
// 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)
{
    if (!fIsZombie)
        fNetwork->SendPDO1(fId, m0, m1, m2, m3, m4, m5, m6, m7);
    return !fIsZombie;
}

// --------------------------------------------------------------------------
//
// Sends the given PDO2 through the network to this device
// A PDO is carrying up to eight bytes of information.
//
// 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)
{
    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;
}

// --------------------------------------------------------------------------
//
// Request a SDO for a given idx/subidx
// An SDO message contains
//  an address (this device)
//  an index of the dictionary entry to read
//  a subindex of this dictionary entry to access
//
// 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;
}

// --------------------------------------------------------------------------
//
// Send a NMT message (command) to this device
//
// 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;
}

// --------------------------------------------------------------------------
//
// Send a Nodeguard message (command) to this device
//
void NodeDrv::SendNodeguard()
{
    fNetwork->SendNodeguard(fId);
}

// --------------------------------------------------------------------------
//
// Enable passthrough for the given functioncode of this device
//
void NodeDrv::EnableCanMsg(BYTE_t fcode)
{
    fNetwork->EnableCanMsg(fId, fcode, TRUE);
}

// --------------------------------------------------------------------------
//
// Wait a given timeout until the SDO with the given idx/subidx from
// this device has been received.
// 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, fIsZombie?-1:timeout);

    if (!rc)
    {
        lout << "NodeDrv::WaitForSdo: 0x" << hex << idx << "/" << dec << (int)subidx << " " << GetNodeName() << " --> ZOMBIE!" << endl;
        SetZombie();
    }
/*
    if (HasError())
    {
        lout << "NodeDrv::WaitForSdo: HasError 0x" << hex << idx << "/" << dec << (int)subidx << " " << GetNodeName() << " --> ZOMBIE!" << endl;
        fIsZombie = kTRUE;
    }
*/
    return fIsZombie ? false : rc;
}

/*
void NodeDrv::WaitForSdos()
{
    while (fNetwork->WaitingForSdo(fId))
        usleep(1);
}
*/

// --------------------------------------------------------------------------
//
// Waits until the next Pdo1 from this device has been received
//
void NodeDrv::WaitForNextPdo1()
{
    fNetwork->WaitForNextPdo1(fId);
}

// --------------------------------------------------------------------------
//
// Waits until the next Pdo2 from this device has been received
//
void NodeDrv::WaitForNextPdo2()
{
    fNetwork->WaitForNextPdo2(fId);
}

// --------------------------------------------------------------------------
//
// Waits until the next Pdo3 from this device has been received
//
void NodeDrv::WaitForNextPdo3()
{
    fNetwork->WaitForNextPdo3(fId);
}

// --------------------------------------------------------------------------
//
// Waits until the next Pdo4 from this device has been received
//
void NodeDrv::WaitForNextPdo4()
{
    fNetwork->WaitForNextPdo4(fId);
}

// --------------------------------------------------------------------------
//
// Start the standard CANopen guarding of the device.
// While ms is the guard time in millisec. This is the time between
// two requests for a Nodeguard message.
// ltf is the LifeTimeFactor. This means how often it is checked, that at
// least one Nodeguard message was answered.
//
void NodeDrv::StartGuarding(Bool_t real=kTRUE)
{
    if (fTimerOn)
        return;

    if (!real)
        SendNodeguard();

    fTimerOn = kTRUE;
    fTimeout->SetTime(fGuardTime);
    fTimeout->Reset();

    Timer t;
    fTimeoutTime = t.Now() + (fGuardTime*fLifeTimeFactor/1000.);
    //cout << GetNodeName() << ": " << fmod(fTimeoutTime*10000, 10000)/10 << endl;

    fTimeout->TurnOn();
    //fTimeout->Start(fGuardTime, kTRUE);

    lout << "- " << GetNodeName() << ": Guarding (" << dec;
    lout << fLifeTimeFactor << "*" << fGuardTime << "ms) started." << endl;
}

void NodeDrv::StartGuarding(Int_t ms, Int_t ltf, Bool_t real)
{
    if (fTimerOn)
    {
        lout << "- " << GetNodeName() << ": ERROR - Guarding already started." << endl;
        return;
    }
    fGuardTime      = ms;
    fLifeTimeFactor = ltf;

    StartGuarding(real);
}

void NodeDrv::StopGuarding()
{
    if (!fTimerOn)
        return;

    fTimeout->TurnOff();
    fTimerOn = kFALSE;

    lout << "- " << GetNodeName() << ": Guarding stopped." << endl;
}

// --------------------------------------------------------------------------
//
// Handle the Nodeguard-Timer Event.
// It checks whether the node timed out. If it timed out it is set to
// the Zombie state.
// A new Nodeguard request is send and a new timer event is triggered.
//
Bool_t NodeDrv::HandleTimer(TTimer *t)
{
    //
    // WARNING:
    //           It seems, that you should never access ANY output from
    //           here. Neither the GUI, nor COUT. This can result in
    //           'unexpected async reply'
    //

   /*
     Fons:
     -----

     timers never trigger at the same time or when in a TTimer::Notify.
     Little explanation:

     - there are two types of timers synchronous and a-synchronous.
     - synchronous timers are only handled via the ROOT eventloop
       (see TUnixSystem::DispatchOneEvent()). If there are no mouse/keyboard
       events then the synchronous timer queue is checked. So if the processing
       of a mouse/keyboard event takes a long time synchronous timers are not
       called for a while. To prevent this from happening one can call in long
       procedures gSystem->ProcessEvents(). The system schedules only the
       next timer in the queue when the current one's Notify() has finished.
     - a-synchronous timers are triggered via SIGALARM, i.e. the program is
       interupted and execution jumps to the Notify() function. When the
       notify is finished the next a-sync timer is scheduled and the system
       resumes from the place where it was initially interrupted. One of the
       things to remember when using a-sync timers is don't make any graphics
       calls in them. X11 is not re-entrant and it might be that the SIGALARM
       signal interrupted the system while being in X11. A-sync timers are best
       used to set flags that you can test at a convenient and controlled
       time.
       */
    if (fIsZombie)
        return kTRUE;

    Timer time;
    Double_t now = time.Now();
    if (now > fTimeoutTime)
    {
        cout << GetNodeName() << ": " << "==out==> " << fmod(now*1000, 10000)/10 << " > " << fmod(fTimeoutTime*10000, 10000)/10 << endl;
        //cout << "ERROR - " << GetNodeName() << " didn't respond in timeout window." << endl;
        //lout << "ERROR - " << GetNodeName() << " didn't respond in timeout window." << endl;
        //cout << dec << "+" << (int)GetId() << ": Handle: " << fmod(now, 500) << endl;
        //cout << dec << "+" << (int)GetId() << ": Handle: " << fmod(fTimeoutTime, 500) << endl;
        //cout << fGuardTime << endl;
        fIsZombie = true;
        //SetZombie();

        return kTRUE;
    }

    SendNodeguard();

    return kTRUE;
}

// --------------------------------------------------------------------------
//
// Set the timeout timer to the time the event was received plus the
// guard time times lifetimefactor.
//
void NodeDrv::HandleNodeguard(timeval_t *tv)
{
    Timer t(tv);
    fTimeoutTime = t + (fGuardTime*fLifeTimeFactor/1000.);
    //cout << GetNodeName() << ": " << fmod(fTimeoutTime*10000, 10000)/10 << endl;
}

void NodeDrv::SetZombie()
{
    fIsZombie = true;
    StopGuarding();
}