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

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

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

#include <iostream.h> // cout
#include <iomanip.h>  // setw, setfill

ClassImp(Network);

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

    VmodIcan::Start();
    InitNodes();

    lout << "- Network started." << endl;
}

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

    StopNodes();
    VmodIcan::Stop();

    lout << "- Network stopped." << endl;
}

// --------------------------------------------------------------------------
//
// Initialize the network, set all nodes to NULL (n/a)
//
Network::Network(const char *dev, const int baud, MLog &out) : CanOpen(dev, baud, out)
{
    for (int i=0; i<32; i++)
        fNodes[i] = NULL;

    lout << "- 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, 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(data);
            return;
        }

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

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

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

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

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

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

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

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

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

// --------------------------------------------------------------------------
//
// Distributes Nodeguard messages to the correspoding node calling
// HandleNodeguard
//
void Network::HandleNodeguard(BYTE_t node, timeval_t *tv)
{
    if (!fNodes[node])
    {
        cout << "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, timeval_t *tv)
{
    if (!fNodes[node])
    {
        cout << "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)
    {
        cout << "SetNode - Error: 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])
        {
            lout << "- Setting up Node #" << dec << i << " (";
            lout << fNodes[i]->GetNodeName() << ")" << endl;
            if (fNodes[i]->InitDevice(this))
                fNodeInitialized[i] = TRUE;
            else
                lout << "- " << fNodes[i]->GetNodeName() << ": InitDevice failed." << endl;
        }
    lout << "- All Nodes setup." << endl;
}

// --------------------------------------------------------------------------
//
// Stop all nodes calling StopDevice
//
void Network::StopNodes()
{
    for (int i=0; i<32; i++)
        if (fNodes[i] && fNodeInitialized[i])
        {
            lout << "- Stopping Node #" << dec << i;
            lout << " (" << fNodes[i]->GetNodeName() << ")" << endl;
            fNodes[i]->StopDevice();
        }
    lout << "- 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;

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

        if (fNodes[i]->GetError() <= 0)
            lout << "Error occured." << endl;
        else
            lout << "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 (!fNodes[i]->HasError())
            continue;

        rc = true;

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

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

    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()
{
    bool rc = true;

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

    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();
}