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

/* ======================================================================== *\
!
! *
! * 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>
#include <iostream>

//#include <TTimer.h>

#include "MTime.h"
#include "network.h"
#include "MLogManip.h"

ClassImp(NodeDrv);

using namespace std;

// --------------------------------------------------------------------------
//
// 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), fGuard(NULL)
{
    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;
    }

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

}

// --------------------------------------------------------------------------
//
// destructor
//
NodeDrv::~NodeDrv()
{
}

// --------------------------------------------------------------------------
//
// 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.
// In standard CANOpen operation data is meaningless (we are using
// it in the 'non-standard' CANOpen communication with the MACS)
//
void NodeDrv::HandleSDOOK(WORD_t idx, BYTE_t subidx, LWORD_t data, 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, BYTE_t m1, BYTE_t m2, BYTE_t m3,
                       BYTE_t m4, BYTE_t m5, BYTE_t m6, BYTE_t m7)
{
    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, BYTE_t m1, BYTE_t m2, BYTE_t m3,
                       BYTE_t m4, BYTE_t m5, BYTE_t m6, BYTE_t m7)
{
    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 zombie)
{
    bool rc = fNetwork->WaitForSdo(fId, idx, subidx, fIsZombie?-1:timeout);
    if (rc)
        return true;

    lout << " + " << GetNodeName() << ": NodeDrv::WaitForSdo: 0x" << hex << idx << "/" << dec << (int)subidx << " --> ZOMBIE! " << MTime(-1) << endl;
    if (zombie)
        SetZombie();
    return false;
}

/*
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.
//
class NodeGuard : public MThread
{
    Double_t fTimeoutTime;     //[s]
    Double_t fGuardTime;       //[s]
    Int_t    fLifeTimeFactor;

    Bool_t   fIsCanOpen;

    NodeDrv *fDrv;

public:
    NodeGuard(NodeDrv *drv, Int_t guard, Int_t ltf, Bool_t canopen)
        : MThread(false), fGuardTime(guard/1000.), fLifeTimeFactor(ltf), fIsCanOpen(canopen), fDrv(drv) { }

    void Reset(timeval_t *tv=NULL)
    {
        MTime t;
        if (tv)
            t.Set(*tv);
        else
            t.Now();

        fTimeoutTime = t + (fGuardTime*fLifeTimeFactor);
    }

    void *Thread()
    {
        Reset();

        while (!HasStopFlag())
        {
            // Sending nodeguards seems to result in
            // loosing CANbus messages or CANbus answers...
            // strange. Also protecting VmodIcan::SendCanFrame
            // by a Mutex doesn't help.
            if (fIsCanOpen)
                fDrv->SendNodeguard();

            MTime t;
            t.Now();

            const Double_t t0 = t+fGuardTime;

            while (!HasStopFlag() && (double)t<t0 && (double)t<fTimeoutTime)
            {
                usleep(5);
                t.Now();
            }

            //cout << "-d-> " << (Long_t)((fTimeoutTime-t)*1000) << endl;
            //cout << "-o-> " << (ULong_t)((fTimeoutTime)*1000)<< " " << (ULong_t)((t)*1000) << endl;
            //cout << "-g-> " << (Long_t)((t-t0)*1000)<< endl;

            if ((double)t<fTimeoutTime)
                continue;

            fDrv->SetZombie(false);
            return 0;
        }
        return 0;
    }
};

void NodeDrv::StartGuarding(Bool_t real)
{
    if (fGuard)
        return;

    fGuard = new NodeGuard(this, fGuardTime, fLifeTimeFactor, real);
    fGuard->Start();

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

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

    fGuardTime      = ms;
    fLifeTimeFactor = ltf;

    StartGuarding(real);
}

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

    delete fGuard;
    fGuard=NULL;

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

// --------------------------------------------------------------------------
//
// Set the timeout timer to the time the event was received plus the
// guard time times lifetimefactor.
//
void NodeDrv::HandleNodeguard(timeval_t *tv)
{
    if (fGuard)
        fGuard->Reset(tv);
}

void NodeDrv::SetZombie(bool stopguard)
{
    fIsZombie = true;
    if (stopguard)
        StopGuarding();
    else
        lout << " - " << GetNodeName() << ": Zombie set due to timeout." << endl;
}