source: trunk/MagicSoft/Cosy/candrv/network.cc@ 9488

/* ======================================================================== *\
!
! *
! * 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, 2001 <mailto:tbretz@astro.uni-wuerzburg.de>
!
!   Copyright: MAGIC Software Development, 2000-2008
!
!
\* ======================================================================== */

///////////////////////////////////////////////////////////////////////
//
// Network
//
// is a collection of nodes which coordinates the network
//
///////////////////////////////////////////////////////////////////////
#include "network.h"

#include "MLog.h"
#include "MLogManip.h"

ClassImp(Network);

using namespace std;

// --------------------------------------------------------------------------
//
// Start the canopen module
// Initialize all nodes (calling NodeDrv::Init()
//
void Network::Start()
{
    gLog << inf << "- Starting network." << endl;

    CanOpen::Start();
    InitNodes();

    gLog << inf << "- Network started." << endl;
}

// --------------------------------------------------------------------------
//
// Stop all nodes, stop the can module
//
void Network::Stop()
{
    gLog << inf << "- Stopping network." << endl;

    StopNodes();
    CanOpen::Stop();

    gLog << inf << "- Network stopped." << endl;
}

// --------------------------------------------------------------------------
//
// Initialize the network, set all nodes to NULL (n/a)
//
Network::Network() : CanOpen()
{
    memset(fNodes,           0, 32*sizeof(*fNodes));
    memset(fNodeInitialized, 0, 32*sizeof(*fNodeInitialized));

    gLog << inf << "- Network initialized." << endl;
}

// --------------------------------------------------------------------------
//
// Distributes the received SDO messages to the addressed nodes.
// Depending on the received command either HandleSDO, HandleSDOOK or
// HandleSDOError called.
//  HandleSDO:      Handles a received value
//  HandleSDOOK:    Handles the acknoledgment of a trasmitted SDO
//  HandleSDOError: Handles error occursion (see CanOpen standard)
//
void Network::HandleSDO(BYTE_t node, BYTE_t cmd, WORD_t idx, BYTE_t subidx, LWORD_t data, const timeval_t &tv)
{
    if (fNodes[node])
        switch (cmd)
        {
        case kSDO_TX4:       // answer to 0x40 with 4 bytes of data
            fNodes[node]->HandleSDO(idx, subidx, data, tv);
            return;

        case kSDO_TX3:       // answer to 0x40 with 2 bytes of data
            fNodes[node]->HandleSDO(idx, subidx, data&0xffff, tv);
            return;

        case kSDO_TX1:       // answer to 0x40 with 1 byte  of data
            fNodes[node]->HandleSDO(idx, subidx, data&0xff, tv);
            return;

        case kSDO_TX_OK:     // answer to a SDO_TX message
            fNodes[node]->HandleSDOOK(idx, subidx, data, tv);
            return;

        case kSDO_TX_ERROR:  // error message (instead of 0x60)
            fNodes[node]->HandleSDOError(idx, subidx);
            return;
        }

    gLog << warn << dec << setfill('0');
    gLog << "Network::HandleSDO: Node=" << (int)node  << " Cmd=0x" << hex << (int)cmd << ": ";
    gLog << "Sdo=" << idx  << "/" << (int)subidx << ": 0x" << setw(8) << data;
    gLog << endl;
}

// --------------------------------------------------------------------------
//
// Distributes PDO1 messages to the correspoding node calling HandlePDO1
//
void Network::HandlePDO1(BYTE_t node, const BYTE_t *data, const timeval_t &tv)
{
    if (!fNodes[node])
    {
        gLog << err << "ERROR - Network::HandlePDO1: Node #" << dec << (int)node << " not found - PDO1: " << hex;
        for (int i=0; i<8; i++)
            gLog << " 0x" << (int)data[i];
        gLog << dec << endl;
        return;
    }

    fNodes[node]->HandlePDO1(data, tv);
}

// --------------------------------------------------------------------------
//
// Distributes PDO2 messages to the correspoding node calling HandlePDO2
//
void Network::HandlePDO2(BYTE_t node, const BYTE_t *data, const timeval_t &tv)
{
    if (!fNodes[node])
    {
        gLog << err << "ERROR - Network::HandlePDO2: Node #" << dec << (int)node << " not found - PDO2: " << hex;
        for (int i=0; i<8; i++)
            gLog << " 0x" << (int)data[i];
        gLog << dec << endl;
        return;
    }

    fNodes[node]->HandlePDO2(data, tv);
}

// --------------------------------------------------------------------------
//
// Distributes PDO3 messages to the correspoding node calling HandlePDO3
//
void Network::HandlePDO3(BYTE_t node, const BYTE_t *data, const timeval_t &tv)
{
    if (!fNodes[node])
    {
        gLog << err << "ERROR - Network::HandlePDO3: Node #" << dec << (int)node << " not found - PDO3: " << hex;
        for (int i=0; i<8; i++)
            gLog << " 0x" << (int)data[i];
        gLog << dec << endl;
        return;
    }

    fNodes[node]->HandlePDO3(data, tv);
}

// --------------------------------------------------------------------------
//
// Distributes PDO4 messages to the correspoding node calling HandlePDO4
//
void Network::HandlePDO4(BYTE_t node, const BYTE_t *data, const timeval_t &tv)
{
    if (!fNodes[node])
    {
        gLog << err << "ERROR - Network::HandlePDO4: Node #" << dec << (int)node << " not found - PDO4: " << hex;
        for (int i=0; i<8; i++)
            gLog << " 0x" << (int)data[i];
        gLog << dec << endl;
        return;
    }

    fNodes[node]->HandlePDO4(data, tv);
}

// --------------------------------------------------------------------------
//
// Distributes Nodeguard messages to the correspoding node calling
// HandleNodeguard
//
void Network::HandleNodeguard(BYTE_t node, const timeval_t &tv)
{
    if (!fNodes[node])
    {
        gLog << err << "ERROR - Network::HandleNodeguard: Node #" << dec << (int)node << " not found: Nodeguard." << endl;
        return;
    }

    fNodes[node]->HandleNodeguard(tv);
}

// --------------------------------------------------------------------------
//
// Distributes Emergency messages to the correspoding node calling
// HandleEmergency
//
void Network::HandleEmergency(BYTE_t node, const timeval_t &tv)
{
    if (!fNodes[node])
    {
        gLog << err << "ERROR - Network::HandleEmergency: Node #" << dec << (int)node << " not found: Emergency." << endl;
        return;
    }

    fNodes[node]->HandleEmergency(tv);
}


// --------------------------------------------------------------------------
//
// Sets a node to a given nodedrv. The id is requested from the drv object.
//
void Network::SetNode(NodeDrv *drv)
{
    const BYTE_t nodeid = drv->GetId();

    if (nodeid>31)
    {
        gLog << err << "ERROR - Network::SetNode: Only node Numbers < 32 are allowed"<< endl;
        return;
    }

    fNodes[nodeid] = drv;
}

// --------------------------------------------------------------------------
//
// Initializes all nodes calling InitDevice
//
void Network::InitNodes()
{
    for (int i=0; i<32; i++)
        if (fNodes[i])
        {
            gLog << inf2 << "- Setting up Node #" << dec << i << " (";
            gLog << fNodes[i]->GetNodeName() << ")" << endl;
            if (fNodes[i]->InitDevice(this))
                fNodeInitialized[i] = TRUE;
            else
                gLog << err << "- " << fNodes[i]->GetNodeName() << ": InitDevice failed." << endl;
        }
    gLog << inf << "- All Nodes setup." << endl;
}

// --------------------------------------------------------------------------
//
// Stop all nodes calling StopDevice
//
void Network::StopNodes()
{
    for (int i=0; i<32; i++)
        if (fNodes[i] && fNodeInitialized[i])
        {
            gLog << inf2 << "- Stopping Node #" << dec << i;
            gLog << " (" << fNodes[i]->GetNodeName() << ")" << endl;
            fNodes[i]->StopDevice();
        }
    gLog << inf << "- All Nodes stopped." << endl;
}

// --------------------------------------------------------------------------
//
// Print all errors which are currently set
//
void Network::PrintError() const
{
    for (int i=0; i<32; i++)
    {
        if (!fNodes[i])
            continue;

        if (!fNodes[i]->HasError())
            continue;

        gLog << err << "- Node #" << dec << i << " '" << fNodes[i]->GetNodeName() << "' ";

        if (fNodes[i]->GetError() <= 0)
            gLog << "Error occured." << endl;
        else
            gLog << "has error #" << fNodes[i]->GetError() << endl;
    }
}

// --------------------------------------------------------------------------
//
// returns true if one of the nodes has the error-flag set (HasError).
//
bool Network::HasError() const
{
    bool rc = false;

    for (int i=0; i<32; i++)
    {
        if (!fNodes[i])
            continue;

        if (CanOpen::HasError())
            fNodes[i]->SetZombie();

        if (!fNodes[i]->HasError())
            continue;

        rc = true;

        if (fNodes[i]->GetError() <= 0)
            continue;

        //gLog << "- Node #" << dec << i << " '" << fNodes[i]->GetNodeName();
        //gLog << "' has error #" << fNodes[i]->GetError() << endl;
    }

    if (CanOpen::HasError())
        return true;

    return rc;
}

// --------------------------------------------------------------------------
//
// returns true if one of the nodes is a zombie node
//
bool Network::HasZombie() const
{
    for (int i=0; i<32; i++)
        if (fNodes[i])
            if (fNodes[i]->IsZombieNode())
                return true;

    return false;
}

// --------------------------------------------------------------------------
//
// try to reboot all zombie nodes to get them working again. all other
// nodes are left untouched.
//
bool Network::RebootZombies()
{
    if (!HasConnection())
    {
        gLog << warn << "- No connection to network." << endl;
        return false;
    }

    bool rc = true;

    gLog << inf2 << "- Trying to reboot all Zombies..." << endl;
    for (int i=0; i<32; i++)
        if (fNodes[i])
            if (fNodes[i]->IsZombieNode())
                if (!fNodes[i]->Reboot())
                {
                    gLog << err << "- Failed to reboot " << fNodes[i]->GetNodeName() << "." << endl;
                    rc = false;
                }

//    if (rc)
//        gLog << inf << "- 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();
}